You are an expert in writing code in many different languages. Your goal is to perform code completion for the following code, keeping in mind the rest of the code and the file meta data.
    Metadata details:
    filename: poly2nodez.m
    path of file: ./repos/Robotics-Toolbox/rvcdlcs/src/planning/polytope
    
    the code of the file until where you have to start completion: function [z,exit] = poly2nodez(polytope,rloc,config,envir,robotnum,dispbaseref,dispendref)
% Find a feasible node z in one o
    
function [c,ceq] = safecon(z,Ap,bp,robotnum,dispbaseref,dispendref)

% get rloc and config (siz)
rloc = z(1:2);
siz = z(3);
% you may change ang according to rloc here
% get displacement base
rob_vts = zeros(robotnum,2);
for i=1:robotnum
    rob_vts(i,:) = rloc+(dispend