3 lines
9.9 KiB
JavaScript
3 lines
9.9 KiB
JavaScript
const F="https://text-to-cad.dev/srdf",$="https://text-to-cad.dev/explorer";function R(n){return!!n&&typeof n=="object"&&!Array.isArray(n)}function D(n){return String(n?.localName||n?.tagName||"").split(":").pop()}function h(n,e){return Array.from(n?.childNodes||[]).filter(i=>i?.nodeType===1&&D(i)===e)}function k(n,e){const i=new Set;return n.map(s=>String(s||"").trim()).filter(s=>{if(!s)throw new Error(`${e} cannot include empty values`);if(i.has(s))throw new Error(`${e} includes duplicate ${s}`);return i.add(s),!0})}function y(n,e){const i=new Set(n);for(const s of e)i.has(s)||(n.push(s),i.add(s))}function j(n,e,i){if(!e||!i||e===i)return[];const s=Array.isArray(n?.joints)?n.joints:[],u=new Map;for(const t of s){const r=String(t?.parentLink||"").trim(),a=String(t?.childLink||"").trim();if(!r||!a)continue;const m=u.get(r)||[];m.push(t),u.set(r,m)}const c=[[e,[]]],o=new Set;for(;c.length;){const[t,r]=c.pop();if(t===i)return r;if(!o.has(t)){o.add(t);for(const a of[...u.get(t)||[]].reverse()){const m=String(a?.childLink||"").trim(),p=String(a?.name||"").trim();m&&p&&c.push([m,[...r,p]])}}}return[]}function E(n,e,i,s=new Set){const u=new Map((Array.isArray(e?.joints)?e.joints:[]).map(r=>[String(r?.name||""),r]).filter(([r])=>r)),c=[],o=Array.isArray(n?.jointNames)?n.jointNames:[];if(o.length)return o.filter(r=>{const a=u.get(String(r||""));return a&&String(a.type||"")!=="fixed"&&!a.mimic});for(const r of Array.isArray(n?.chains)?n.chains:[])y(c,j(e,String(r?.baseLink||""),String(r?.tipLink||"")).filter(a=>{const m=u.get(a);return m&&String(m.type||"")!=="fixed"&&!m.mimic}));const t=String(n?.name||"");t&&s.add(t);for(const r of Array.isArray(n?.subgroups)?n.subgroups:[]){const a=String(r||"").trim();if(!a||s.has(a))continue;const m=i.get(a);m&&y(c,E(m,e,i,s))}return c}function A(n,e,i,s=new Set){const u=new Map((Array.isArray(e?.joints)?e.joints:[]).map(r=>[String(r?.name||""),r]).filter(([r])=>r)),c=new Set(Array.isArray(n?.linkNames)?n.linkNames.map(r=>String(r||"")).filter(Boolean):[]),o=r=>{const a=u.get(String(r||"")),m=String(a?.childLink||"").trim();m&&c.add(m)};for(const r of Array.isArray(n?.jointNames)?n.jointNames:[])o(r);for(const r of Array.isArray(n?.chains)?n.chains:[]){const a=String(r?.baseLink||"").trim(),m=String(r?.tipLink||"").trim();m&&c.add(m);for(const p of j(e,a,m))o(p)}const t=String(n?.name||"");t&&s.add(t);for(const r of Array.isArray(n?.subgroups)?n.subgroups:[]){const a=String(r||"").trim();if(!a||s.has(a))continue;const m=i.get(a);if(m)for(const p of A(m,e,i,s))c.add(p)}return c}function L(n,e,i){return(Array.isArray(n?.joints)?n.joints:[]).some(u=>{const c=String(u?.parentLink||"").trim(),o=String(u?.childLink||"").trim();return c===e&&i.has(o)||o===e&&i.has(c)})}function _(n){const e=String(n||"").trim().toLowerCase();return e.includes("adjacent")?"adjacent":["never","always","sample","default"].some(i=>e.includes(i))?"sampled":e.includes("setup")||e.includes("assistant")?"setup_assistant":e.includes("assum")?"assumed":"manual"}function P(n){const e=h(n,"urdf").find(s=>s.namespaceURI===F||s.namespaceURI===$||String(s.tagName||"").startsWith("tcad:")||String(s.tagName||"").startsWith("explorer:")),i=String(e?.getAttribute("path")||"").trim();if(!i)throw new Error('SRDF must include <tcad:urdf path="..."/> metadata; legacy <explorer:urdf/> is also accepted');if(i.includes("\\")||i.startsWith("/")||i.split("/").some(s=>!s||s==="."))throw new Error(`SRDF tcad:urdf path is invalid: ${i}`);if(!i.toLowerCase().endsWith(".urdf"))throw new Error(`SRDF tcad:urdf path must end in .urdf: ${i}`);return i}function M(n,e){const i=new Set((Array.isArray(e?.links)?e.links:[]).map(o=>String(o?.name||"")).filter(Boolean)),s=new Set((Array.isArray(e?.joints)?e.joints:[]).map(o=>String(o?.name||"")).filter(Boolean)),u=new Set,c=h(n,"group").map(o=>{const t=String(o.getAttribute("name")||"").trim();if(!t)throw new Error("SRDF planning group name is required");if(u.has(t))throw new Error(`Duplicate SRDF planning group: ${t}`);u.add(t);const r=k(h(o,"joint").map(l=>l.getAttribute("name")),`SRDF planning group ${t} jointNames`),a=k(h(o,"link").map(l=>l.getAttribute("name")),`SRDF planning group ${t} linkNames`);for(const l of r)if(!s.has(l))throw new Error(`SRDF planning group ${t} references missing joint ${l}`);for(const l of a)if(!i.has(l))throw new Error(`SRDF planning group ${t} references missing link ${l}`);const m=h(o,"chain").map(l=>{const g=String(l.getAttribute("base_link")||"").trim(),d=String(l.getAttribute("tip_link")||"").trim();if(!i.has(g)||!i.has(d))throw new Error(`SRDF planning group ${t} chain references missing link`);return{baseLink:g,tipLink:d}}),p=k(h(o,"group").map(l=>l.getAttribute("name")),`SRDF planning group ${t} subgroups`);return{name:t,jointNames:r,linkNames:a,chains:m,subgroups:p}});if(!c.length)throw new Error("SRDF must define at least one planning group");for(const o of c)for(const t of o.subgroups)if(!u.has(t))throw new Error(`SRDF planning group ${o.name} references missing subgroup ${t}`);return c}function v(n){return Array.isArray(n?.linkNames)&&n.linkNames.length?String(n.linkNames[n.linkNames.length-1]||""):Array.isArray(n?.chains)&&n.chains.length?String(n.chains[n.chains.length-1]?.tipLink||""):""}function x(n,e){return v(e.get(n.group))||n.parentLink}function B(n,e,i){const s=new Set((Array.isArray(e?.links)?e.links:[]).map(o=>String(o?.name||"")).filter(Boolean)),u=new Map(i.map(o=>[o.name,o])),c=new Set;return h(n,"end_effector").map(o=>{const t=String(o.getAttribute("name")||"").trim();if(!t)throw new Error("SRDF end effector name is required");if(c.has(t))throw new Error(`Duplicate SRDF end effector: ${t}`);c.add(t);const r=String(o.getAttribute("parent_link")||"").trim(),a=String(o.getAttribute("group")||"").trim(),m=String(o.getAttribute("parent_group")||"").trim();if(!s.has(r))throw new Error(`SRDF end effector ${t} references missing parent_link ${r||"(missing)"}`);if(!u.has(a))throw new Error(`SRDF end effector ${t} references missing group ${a||"(missing)"}`);if(m&&!u.has(m))throw new Error(`SRDF end effector ${t} references missing parent_group ${m}`);const p={name:t,parentLink:r,group:a,parentGroup:m},l=x(p,u);if(!s.has(l))throw new Error(`SRDF end effector ${t} references missing link ${l||"(missing)"}`);const g=A(u.get(a),e,u);if(m){const d=A(u.get(m),e,u),f=[...g].filter(w=>d.has(w)).sort();if(f.length)throw new Error(`SRDF end effector ${t} group shares link(s) with parent_group: ${f.join(", ")}`);if(!d.has(r))throw new Error(`SRDF end effector ${t} parent_link is not in parent_group ${m}`)}if(g.size&&!g.has(r)&&!L(e,r,g))throw new Error(`SRDF end effector ${t} parent_link is not adjacent to its group`);return{...p,link:l}})}function C(n,e,i){const s=new Set(i.map(t=>t.name)),u=new Map(i.map(t=>[t.name,t])),c=new Map((Array.isArray(e?.joints)?e.joints:[]).map(t=>[String(t?.name||""),t]).filter(([t])=>t)),o=new Set;return h(n,"group_state").map(t=>{const r=String(t.getAttribute("name")||"").trim(),a=String(t.getAttribute("group")||"").trim();if(!r||!s.has(a))throw new Error(`SRDF group_state ${r||"(missing)"} references missing group ${a||"(missing)"}`);const m=`${a}/${r}`;if(o.has(m))throw new Error(`Duplicate SRDF group_state ${m}`);o.add(m);const p=new Set(E(u.get(a),e,u)),l=new Set,g={};for(const d of h(t,"joint")){const f=String(d.getAttribute("name")||"").trim(),w=Number(d.getAttribute("value")),S=c.get(f);if(!S||!Number.isFinite(w))throw new Error(`SRDF group_state ${r} has invalid joint value ${f||"(missing)"}`);if(l.has(f))throw new Error(`SRDF group_state ${r} includes duplicate joint ${f}`);if(l.add(f),!p.has(f))throw new Error(`SRDF group_state ${r} joint ${f} is not in group ${a}`);if(String(S.type||"")==="fixed"||S.mimic)throw new Error(`SRDF group_state ${r} cannot set fixed or mimic joint ${f}`);if(String(S.type||"")!=="continuous"){const b=String(S.type||"")==="revolute"?Number(S.minValueDeg)*(Math.PI/180):Number(S.minValueDeg),N=String(S.type||"")==="revolute"?Number(S.maxValueDeg)*(Math.PI/180):Number(S.maxValueDeg);if(Number.isFinite(b)&&w<b)throw new Error(`SRDF group_state ${r} joint ${f} is below its URDF lower limit`);if(Number.isFinite(N)&&w>N)throw new Error(`SRDF group_state ${r} joint ${f} is above its URDF upper limit`)}g[f]=w}return{name:r,group:a,jointValuesByName:g,jointValuesByNameRad:g}})}function G(n,e){const i=new Set((Array.isArray(e?.links)?e.links:[]).map(u=>String(u?.name||"")).filter(Boolean)),s=new Set;return h(n,"disable_collisions").map(u=>{const c=String(u.getAttribute("link1")||"").trim(),o=String(u.getAttribute("link2")||"").trim(),t=String(u.getAttribute("reason")||"").trim();if(!i.has(c)||!i.has(o))throw new Error("SRDF disabled collision pair references missing link");if(c===o)throw new Error("SRDF disabled collision pair cannot repeat the same link");const r=[c,o].sort().join("/");if(s.has(r))throw new Error(`Duplicate SRDF disabled collision pair ${r}`);if(s.add(r),!t)throw new Error(`SRDF disabled collision pair ${r} requires a reason`);return{link1:c,link2:o,reason:t,source:_(t)}})}function U(n){return R(n)?{transport:"moveit2Server",planningGroups:n.planningGroups,endEffectors:n.endEffectors,groupStates:n.groupStates,disabledCollisionPairs:n.disabledCollisionPairs,srdf:n}:null}function q(n,{sourceUrl:e="",urdfData:i=null}={}){if(typeof DOMParser>"u")throw new Error("DOMParser is unavailable in this environment");const s=new DOMParser().parseFromString(String(n||""),"application/xml");if(s.querySelector("parsererror"))throw new Error("Failed to parse SRDF XML");const c=s.documentElement;if(!c||c.tagName!=="robot")throw new Error("SRDF root element must be <robot>");const o=String(c.getAttribute("name")||"").trim();if(!o)throw new Error("SRDF robot name is required");if(i?.robotName&&i.robotName!==o)throw new Error("SRDF robot name must match the linked URDF robot name");const t=P(c),r=M(c,i),a=B(c,i,r),m=C(c,i,r),p=G(c,i);return{kind:"texttocad-srdf",urdf:t,srdf:e,robotName:o,planningGroups:r,endEffectors:a,groupStates:m,disabledCollisionPairs:p}}export{U as motionFromSrdf,q as parseSrdf};
|
|
//# sourceMappingURL=parseSrdf-5nF_MoHR.js.map
|