diff --git a/src/urdf/UrdfBox.ts b/src/urdf/UrdfBox.ts index 437c45399..05167e714 100644 --- a/src/urdf/UrdfBox.ts +++ b/src/urdf/UrdfBox.ts @@ -4,32 +4,30 @@ * @author Russell Toris - rctoris@wpi.edu */ -import Vector3 from '../math/Vector3.js'; -import * as UrdfTypes from './UrdfTypes.js'; +import { Vector3 } from '../math/index.js'; +import { UrdfAttrs, UrdfType, type UrdfDefaultOptions } from './UrdfTypes.js'; +import type { Optional, Nullable } from '../types/interface-types.js'; /** * A Box element in a URDF. */ export default class UrdfBox { - /** @type {Vector3 | null} */ - dimension; - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.type = UrdfTypes.URDF_BOX; + type: UrdfType; + dimension: Nullable = null; + + constructor({ xml }: UrdfDefaultOptions) { + this.type = UrdfType.BOX; // Parse the xml string - var xyz = options.xml.getAttribute('size')?.split(' '); - if (xyz) { - this.dimension = new Vector3({ - x: parseFloat(xyz[0]), - y: parseFloat(xyz[1]), - z: parseFloat(xyz[2]) - }); - } else { - this.dimension = null; + const size: Optional = xml.getAttribute(UrdfAttrs.Size)?.split(' '); + if (!size || size.length !== 3) { + return; } + + this.dimension = new Vector3({ + x: parseFloat(size[0]), + y: parseFloat(size[1]), + z: parseFloat(size[2]) + }); } } diff --git a/src/urdf/UrdfColor.ts b/src/urdf/UrdfColor.ts index be5ec2821..850171e4c 100644 --- a/src/urdf/UrdfColor.ts +++ b/src/urdf/UrdfColor.ts @@ -4,22 +4,41 @@ * @author Russell Toris - rctoris@wpi.edu */ +import { UrdfAttrs, type UrdfDefaultOptions } from './UrdfTypes.js'; +import type { Optional } from '../types/interface-types.js'; + /** * A Color element in a URDF. */ export default class UrdfColor { + + /** + * Color Red, [0, 1] + */ + r = 0.0; + /** + * Color Green, [0, 1] + */ + g = 0.0; + /** + * Color Blue, [0, 1] + */ + b = 0.0; /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. + * Alpha/Opacity, [0, 1] */ - constructor(options) { + a = 1.0; + + constructor({ xml }: UrdfDefaultOptions) { // Parse the xml string - var rgba = options.xml.getAttribute('rgba')?.split(' '); - if (rgba) { - this.r = parseFloat(rgba[0]); - this.g = parseFloat(rgba[1]); - this.b = parseFloat(rgba[2]); - this.a = parseFloat(rgba[3]); + const rgba: Optional = xml.getAttribute(UrdfAttrs.Rgba)?.split(' '); + if (!rgba || rgba.length !== 4) { + return; } + + this.r = parseFloat(rgba[0]); + this.g = parseFloat(rgba[1]); + this.b = parseFloat(rgba[2]); + this.a = parseFloat(rgba[3]); } } diff --git a/src/urdf/UrdfCylinder.ts b/src/urdf/UrdfCylinder.ts index 84b7a98f3..460bdfff0 100644 --- a/src/urdf/UrdfCylinder.ts +++ b/src/urdf/UrdfCylinder.ts @@ -4,21 +4,21 @@ * @author Russell Toris - rctoris@wpi.edu */ -import * as UrdfTypes from './UrdfTypes.js'; +import { type UrdfDefaultOptions, UrdfType, UrdfAttrs } from './UrdfTypes.js'; /** * A Cylinder element in a URDF. */ export default class UrdfCylinder { - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.type = UrdfTypes.URDF_CYLINDER; - // @ts-expect-error -- possibly null - this.length = parseFloat(options.xml.getAttribute('length')); - // @ts-expect-error -- possibly null - this.radius = parseFloat(options.xml.getAttribute('radius')); + + type: UrdfType; + length: number; + radius: number; + + constructor({ xml }: UrdfDefaultOptions) { + this.type = UrdfType.CYLINDER; + + this.length = parseFloat(xml.getAttribute(UrdfAttrs.Length) ?? 'NaN'); + this.radius = parseFloat(xml.getAttribute(UrdfAttrs.Radius) ?? 'NaN'); } } diff --git a/src/urdf/UrdfJoint.ts b/src/urdf/UrdfJoint.ts index be9ec286b..3b5c2bca3 100644 --- a/src/urdf/UrdfJoint.ts +++ b/src/urdf/UrdfJoint.ts @@ -3,93 +3,49 @@ * @author David V. Lu!! - davidvlu@gmail.com */ -import Pose from '../math/Pose.js'; -import Vector3 from '../math/Vector3.js'; -import Quaternion from '../math/Quaternion.js'; +import { UrdfAttrs, type UrdfDefaultOptions } from './UrdfTypes.js'; +import { Pose } from '../math/index.js'; +import { parseUrdfOrigin } from './UrdfUtils.js'; +import type { Nullable } from '../types/interface-types.js'; /** * A Joint element in a URDF. */ export default class UrdfJoint { - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.name = options.xml.getAttribute('name'); - this.type = options.xml.getAttribute('type'); - var parents = options.xml.getElementsByTagName('parent'); + name: string; + type: Nullable; + parent: Nullable = null; + child: Nullable = null; + minval = NaN; + maxval = NaN; + origin: Pose = new Pose(); + + + constructor({xml}: UrdfDefaultOptions) { + this.name = xml.getAttribute(UrdfAttrs.Name) ?? 'unknown_name'; + this.type = xml.getAttribute(UrdfAttrs.Type); + + const parents = xml.getElementsByTagName(UrdfAttrs.Parent); if (parents.length > 0) { - this.parent = parents[0].getAttribute('link'); + this.parent = parents[0].getAttribute(UrdfAttrs.Link); } - var children = options.xml.getElementsByTagName('child'); + const children = xml.getElementsByTagName(UrdfAttrs.Child); if (children.length > 0) { - this.child = children[0].getAttribute('link'); + this.child = children[0].getAttribute(UrdfAttrs.Link); } - var limits = options.xml.getElementsByTagName('limit'); + const limits = xml.getElementsByTagName(UrdfAttrs.Limit); if (limits.length > 0) { - this.minval = parseFloat(limits[0].getAttribute('lower') || 'NaN'); - this.maxval = parseFloat(limits[0].getAttribute('upper') || 'NaN'); + this.minval = parseFloat(limits[0].getAttribute(UrdfAttrs.Lower) ?? 'NaN'); + this.maxval = parseFloat(limits[0].getAttribute(UrdfAttrs.Upper) ?? 'NaN'); } // Origin - var origins = options.xml.getElementsByTagName('origin'); - if (origins.length === 0) { - // use the identity as the default - this.origin = new Pose(); - } else { - // Check the XYZ - var xyzValue = origins[0].getAttribute('xyz'); - var position = new Vector3(); - if (xyzValue) { - var xyz = xyzValue.split(' '); - position = new Vector3({ - x: parseFloat(xyz[0]), - y: parseFloat(xyz[1]), - z: parseFloat(xyz[2]) - }); - } - - // Check the RPY - var rpyValue = origins[0].getAttribute('rpy'); - var orientation = new Quaternion(); - if (rpyValue) { - var rpy = rpyValue.split(' '); - // Convert from RPY - var roll = parseFloat(rpy[0]); - var pitch = parseFloat(rpy[1]); - var yaw = parseFloat(rpy[2]); - var phi = roll / 2.0; - var the = pitch / 2.0; - var psi = yaw / 2.0; - var x = - Math.sin(phi) * Math.cos(the) * Math.cos(psi) - - Math.cos(phi) * Math.sin(the) * Math.sin(psi); - var y = - Math.cos(phi) * Math.sin(the) * Math.cos(psi) + - Math.sin(phi) * Math.cos(the) * Math.sin(psi); - var z = - Math.cos(phi) * Math.cos(the) * Math.sin(psi) - - Math.sin(phi) * Math.sin(the) * Math.cos(psi); - var w = - Math.cos(phi) * Math.cos(the) * Math.cos(psi) + - Math.sin(phi) * Math.sin(the) * Math.sin(psi); - - orientation = new Quaternion({ - x: x, - y: y, - z: z, - w: w - }); - orientation.normalize(); - } - this.origin = new Pose({ - position: position, - orientation: orientation - }); + const origins = xml.getElementsByTagName(UrdfAttrs.Origin); + if (origins.length > 0) { + this.origin = parseUrdfOrigin(origins[0]); } } } diff --git a/src/urdf/UrdfLink.ts b/src/urdf/UrdfLink.ts index 8bc1e3a5a..44d399884 100644 --- a/src/urdf/UrdfLink.ts +++ b/src/urdf/UrdfLink.ts @@ -5,24 +5,24 @@ */ import UrdfVisual from './UrdfVisual.js'; +import { UrdfAttrs, type UrdfDefaultOptions } from './UrdfTypes.js'; /** * A Link element in a URDF. */ export default class UrdfLink { - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.name = options.xml.getAttribute('name'); - this.visuals = []; - var visuals = options.xml.getElementsByTagName('visual'); - for (var i = 0; i < visuals.length; i++) { + name: string; + visuals: UrdfVisual[] = []; + + constructor({ xml }: UrdfDefaultOptions) { + this.name = xml.getAttribute(UrdfAttrs.Name) ?? 'unknown_name'; + const visuals = xml.getElementsByTagName(UrdfAttrs.Visuals); + + for (const visual of visuals) { this.visuals.push( new UrdfVisual({ - xml: visuals[i] + xml: visual }) ); } diff --git a/src/urdf/UrdfMaterial.ts b/src/urdf/UrdfMaterial.ts index c3fbd3142..c2561358b 100644 --- a/src/urdf/UrdfMaterial.ts +++ b/src/urdf/UrdfMaterial.ts @@ -5,31 +5,30 @@ */ import UrdfColor from './UrdfColor.js'; +import { UrdfAttrs, type UrdfDefaultOptions } from './UrdfTypes.js'; +import type { Nullable } from '../types/interface-types.js'; /** * A Material element in a URDF. */ export default class UrdfMaterial { - /** @type {string | null} */ - textureFilename = null; - /** @type {UrdfColor | null} */ - color = null; - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - - this.name = options.xml.getAttribute('name'); + + name: string; + textureFilename: Nullable = null; + color: Nullable = null; + + constructor({ xml }: UrdfDefaultOptions) { + + this.name = xml.getAttribute(UrdfAttrs.Name) ?? 'unknown_name'; // Texture - var textures = options.xml.getElementsByTagName('texture'); + const textures = xml.getElementsByTagName(UrdfAttrs.Texture); if (textures.length > 0) { - this.textureFilename = textures[0].getAttribute('filename'); + this.textureFilename = textures[0].getAttribute(UrdfAttrs.Filename); } // Color - var colors = options.xml.getElementsByTagName('color'); + const colors = xml.getElementsByTagName(UrdfAttrs.Color); if (colors.length > 0) { // Parse the RBGA string this.color = new UrdfColor({ @@ -37,10 +36,12 @@ export default class UrdfMaterial { }); } } + isLink() { return this.color === null && this.textureFilename === null; } - assign(obj) { + + assign(obj: UrdfMaterial): this & UrdfMaterial { return Object.assign(this, obj); } } diff --git a/src/urdf/UrdfMesh.ts b/src/urdf/UrdfMesh.ts index d4429ec66..26f9fdba5 100644 --- a/src/urdf/UrdfMesh.ts +++ b/src/urdf/UrdfMesh.ts @@ -4,33 +4,36 @@ * @author Russell Toris - rctoris@wpi.edu */ -import Vector3 from '../math/Vector3.js'; -import * as UrdfTypes from './UrdfTypes.js'; +import { Vector3 } from '../math/index.js'; +import { UrdfAttrs, type UrdfDefaultOptions, UrdfType } from './UrdfTypes.js'; +import type { Nullable, Optional } from '../types/interface-types.js'; /** * A Mesh element in a URDF. */ export default class UrdfMesh { - /** @type {Vector3 | null} */ - scale = null; + type: UrdfType; + scale: Nullable = null; + filename: Nullable; + /** * @param {Object} options * @param {Element} options.xml - The XML element to parse. */ - constructor(options) { - this.type = UrdfTypes.URDF_MESH; - this.filename = options.xml.getAttribute('filename'); + constructor({xml}: UrdfDefaultOptions) { + this.type = UrdfType.MESH; + this.filename = xml.getAttribute(UrdfAttrs.Filename); // Check for a scale - var scale = options.xml.getAttribute('scale'); - if (scale) { - // Get the XYZ - var xyz = scale.split(' '); - this.scale = new Vector3({ - x: parseFloat(xyz[0]), - y: parseFloat(xyz[1]), - z: parseFloat(xyz[2]) - }); + const scale: Optional = xml.getAttribute(UrdfAttrs.Scale)?.split(' '); + if (!scale || scale.length !== 3) { + return; } + + this.scale = new Vector3({ + x: parseFloat(scale[0]), + y: parseFloat(scale[1]), + z: parseFloat(scale[2]) + }); } } diff --git a/src/urdf/UrdfModel.ts b/src/urdf/UrdfModel.ts index 3c432d8c6..273843d3f 100644 --- a/src/urdf/UrdfModel.ts +++ b/src/urdf/UrdfModel.ts @@ -4,94 +4,111 @@ * @author Russell Toris - rctoris@wpi.edu */ +import { DOMParser, MIME_TYPE } from '@xmldom/xmldom'; import UrdfMaterial from './UrdfMaterial.js'; import UrdfLink from './UrdfLink.js'; import UrdfJoint from './UrdfJoint.js'; -import { DOMParser, MIME_TYPE } from '@xmldom/xmldom'; +import { isElement } from './UrdfUtils.js'; +import { UrdfAttrs } from './UrdfTypes.js'; // See https://developer.mozilla.org/docs/XPathResult#Constants -var XPATH_FIRST_ORDERED_NODE_TYPE = 9; +// const XPATH_FIRST_ORDERED_NODE_TYPE = 9; + +export interface UrdfModelOptions { + /** + * The XML element to parse. + */ + xml?: Element; + /** + * The XML element to parse as a string. + */ + string: string; +} /** * A URDF Model can be used to parse a given URDF into the appropriate elements. */ export default class UrdfModel { - materials = {}; - links = {}; - joints = {}; - /** - * @param {Object} options - * @param {Element | null} [options.xml] - The XML element to parse. - * @param {string} [options.string] - The XML element to parse as a string. - */ - constructor(options) { - var xmlDoc = options.xml; - var string = options.string; + + name: string | null; + materials: Record = {}; + links: Record = {}; + joints: Record = {}; + + constructor({ xml, string }: UrdfModelOptions) { + let xmlDoc = xml; // Check if we are using a string or an XML element if (string) { // Parse the string - var parser = new DOMParser(); - xmlDoc = parser.parseFromString(string, MIME_TYPE.XML_TEXT).documentElement; + xmlDoc = new DOMParser().parseFromString(string, MIME_TYPE.XML_TEXT).documentElement; } + if (!xmlDoc) { throw new Error('No URDF document parsed!'); } - // Initialize the model with the given XML node. - // Get the robot tag - var robotXml = xmlDoc; - // Get the robot name - this.name = robotXml.getAttribute('name'); + this.name = xmlDoc.getAttribute(UrdfAttrs.Name); + const childNodes = xmlDoc.childNodes; // Parse all the visual elements we need - for (var nodes = robotXml.childNodes, i = 0; i < nodes.length; i++) { - /** @type {Element} */ - // @ts-expect-error -- unknown why this doesn't work properly. - var node = nodes[i]; - if (node.tagName === 'material') { - var material = new UrdfMaterial({ - xml: node - }); - // Make sure this is unique - if (this.materials[material.name] !== void 0) { + for (const node of childNodes) { + + // Safety check to make sure we're working with an element. + if (!isElement(node)) { + continue; + } + + switch (node.tagName) { + case 'material': { + const material = new UrdfMaterial({ xml: node }); + // Make sure this is unique + if (!Object.hasOwn(this.materials, material.name)) { + this.materials[material.name] = material; + break; + } + if (this.materials[material.name].isLink()) { this.materials[material.name].assign(material); } else { - console.warn('Material ' + material.name + 'is not unique.'); + console.warn(`Material ${material.name} is not unique.`); } - } else { - this.materials[material.name] = material; + + break; } - } else if (node.tagName === 'link') { - var link = new UrdfLink({ - xml: node - }); - // Make sure this is unique - if (this.links[link.name] !== void 0) { - console.warn('Link ' + link.name + ' is not unique.'); - } else { + case 'link': { + const link = new UrdfLink({ xml: node }); + // Make sure this is unique + if (Object.hasOwn(this.links, link.name)) { + console.warn(`Link ${link.name} is not unique.`); + break; + } + // Check for a material - for (var j = 0; j < link.visuals.length; j++) { - var mat = link.visuals[j].material; - if (mat !== null && mat.name) { - if (this.materials[mat.name] !== void 0) { - link.visuals[j].material = this.materials[mat.name]; - } else { - this.materials[mat.name] = mat; - } + for (const item of link.visuals) { + const mat = item.material; + if (!mat?.name) { + continue; + } + + if (Object.hasOwn(this.materials, mat.name)) { + item.material = this.materials[mat.name]; + } else { + this.materials[mat.name] = mat; } } // Add the link this.links[link.name] = link; + + break; + } + case 'joint': { + const joint = new UrdfJoint({ xml: node }); + this.joints[joint.name] = joint; + break; } - } else if (node.tagName === 'joint') { - var joint = new UrdfJoint({ - xml: node - }); - this.joints[joint.name] = joint; } } } diff --git a/src/urdf/UrdfSphere.ts b/src/urdf/UrdfSphere.ts index 13c585fba..cb5d39d2e 100644 --- a/src/urdf/UrdfSphere.ts +++ b/src/urdf/UrdfSphere.ts @@ -4,18 +4,18 @@ * @author Russell Toris - rctoris@wpi.edu */ -import * as UrdfTypes from './UrdfTypes.js'; +import { UrdfAttrs, type UrdfDefaultOptions, UrdfType } from './UrdfTypes.js'; /** * A Sphere element in a URDF. */ export default class UrdfSphere { - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - this.type = UrdfTypes.URDF_SPHERE; - this.radius = parseFloat(options.xml.getAttribute('radius') || 'NaN'); + + type: UrdfType; + radius = NaN; + + constructor({xml}: UrdfDefaultOptions) { + this.type = UrdfType.SPHERE; + this.radius = parseFloat(xml.getAttribute(UrdfAttrs.Radius) ?? 'NaN'); } } diff --git a/src/urdf/UrdfTypes.ts b/src/urdf/UrdfTypes.ts index 972d8283a..c649c58c4 100644 --- a/src/urdf/UrdfTypes.ts +++ b/src/urdf/UrdfTypes.ts @@ -1,4 +1,38 @@ -export const URDF_SPHERE = 0; -export const URDF_BOX = 1; -export const URDF_CYLINDER = 2; -export const URDF_MESH = 3; +export enum UrdfType { + SPHERE = 0, + BOX = 1, + CYLINDER = 2, + MESH = 3 +} + +export enum UrdfAttrs { + Name = 'name', + Type = 'type', + Parent = 'parent', + Link = 'link', + Child = 'child', + Limit = 'limit', + Upper = 'upper', + Lower = 'lower', + Origin = 'origin', + Xyz = 'xyz', + Rpy = 'rpy', + Size = 'size', + Rgba = 'rgba', + Length = 'length', + Radius = 'radius', + Visuals = 'visual', + Texture = 'texture', + Filename = 'filename', + Color = 'color', + Geometry = 'geometry', + Material = 'material', + Scale = 'scale', +} + +export interface UrdfDefaultOptions { + /** + * The XML element to parse. + */ + xml: Element; +} diff --git a/src/urdf/UrdfUtils.ts b/src/urdf/UrdfUtils.ts new file mode 100644 index 000000000..43613d229 --- /dev/null +++ b/src/urdf/UrdfUtils.ts @@ -0,0 +1,59 @@ +/********** Utility Methods for parsing Joint **********/ +import { Pose, Quaternion, Vector3 } from '../math/index.js'; +import { UrdfAttrs } from './UrdfTypes.js'; + +export function parseUrdfOrigin(originElement: Element): Pose { + // Check the XYZ + const xyz: string[] | undefined = originElement.getAttribute(UrdfAttrs.Xyz)?.split(' '); + let position: Vector3 = new Vector3(); + if (xyz && xyz.length === 3) { + position = new Vector3({ + x: parseFloat(xyz[0]), + y: parseFloat(xyz[1]), + z: parseFloat(xyz[2]) + }); + } + + // Check the RPY + const rpy = originElement.getAttribute(UrdfAttrs.Rpy)?.split(' '); + let orientation = new Quaternion(); + if (rpy && rpy.length === 3) { + // Convert from RPY + const roll = parseFloat(rpy[0]); + const pitch = parseFloat(rpy[1]); + const yaw = parseFloat(rpy[2]); + const phi = roll / 2.0; + const the = pitch / 2.0; + const psi = yaw / 2.0; + const x = + Math.sin(phi) * Math.cos(the) * Math.cos(psi) - + Math.cos(phi) * Math.sin(the) * Math.sin(psi); + const y = + Math.cos(phi) * Math.sin(the) * Math.cos(psi) + + Math.sin(phi) * Math.cos(the) * Math.sin(psi); + const z = + Math.cos(phi) * Math.cos(the) * Math.sin(psi) - + Math.sin(phi) * Math.sin(the) * Math.cos(psi); + const w = + Math.cos(phi) * Math.cos(the) * Math.cos(psi) + + Math.sin(phi) * Math.sin(the) * Math.sin(psi); + + orientation = new Quaternion({ + x: x, + y: y, + z: z, + w: w + }); + orientation.normalize(); + } + + return new Pose({ + position: position, + orientation: orientation + }); +} + +export function isElement(node: Node): node is Element { + // Node.ELEMENT_TYPE = 1 + return node.nodeType === 1; +} diff --git a/src/urdf/UrdfVisual.ts b/src/urdf/UrdfVisual.ts index 2832a72fb..c00b9262b 100644 --- a/src/urdf/UrdfVisual.ts +++ b/src/urdf/UrdfVisual.ts @@ -5,132 +5,76 @@ */ import Pose from '../math/Pose.js'; -import Vector3 from '../math/Vector3.js'; -import Quaternion from '../math/Quaternion.js'; - import UrdfCylinder from './UrdfCylinder.js'; import UrdfBox from './UrdfBox.js'; import UrdfMaterial from './UrdfMaterial.js'; import UrdfMesh from './UrdfMesh.js'; import UrdfSphere from './UrdfSphere.js'; +import { UrdfAttrs, type UrdfDefaultOptions } from './UrdfTypes.js'; +import { isElement, parseUrdfOrigin } from './UrdfUtils.js'; + +export type UrdfGeometryLike = UrdfMesh | UrdfSphere | UrdfBox | UrdfCylinder; + +function parseUrdfGeometry(geometryElem: Element): UrdfGeometryLike | null { + + let childShape: Element | null = null; + for (const childNode of geometryElem.childNodes) { + if (isElement(childNode)) { + // Safe type check after checking nodeType + childShape = childNode; + break; + } + } + + if (!childShape) { + return null; + } + + const options: UrdfDefaultOptions = { + xml: childShape + } + + switch (childShape.nodeName) { + case 'sphere': + return new UrdfSphere(options); + case 'box': + return new UrdfBox(options); + case 'cylinder': + return new UrdfCylinder(options); + case 'mesh': + return new UrdfMesh(options); + default: + console.warn(`Unknown geometry type ${childShape.nodeName}`); + return null + } +} /** * A Visual element in a URDF. */ export default class UrdfVisual { - /** @type {Pose | null} */ - origin = null; - /** @type {UrdfMesh | UrdfSphere | UrdfBox | UrdfCylinder | null} */ - geometry = null; - /** @type {UrdfMaterial | null} */ - material = null; - /** - * @param {Object} options - * @param {Element} options.xml - The XML element to parse. - */ - constructor(options) { - var xml = options.xml; - this.name = options.xml.getAttribute('name'); + name: string | null; + origin: Pose | null = new Pose(); + geometry: UrdfGeometryLike | null = null; + material: UrdfMaterial | null = null; - // Origin - var origins = xml.getElementsByTagName('origin'); - if (origins.length === 0) { - // use the identity as the default - this.origin = new Pose(); - } else { - // Check the XYZ - var xyzValue = origins[0].getAttribute('xyz'); - var position = new Vector3(); - if (xyzValue) { - var xyz = xyzValue.split(' '); - position = new Vector3({ - x: parseFloat(xyz[0]), - y: parseFloat(xyz[1]), - z: parseFloat(xyz[2]) - }); - } - - // Check the RPY - var rpyValue = origins[0].getAttribute('rpy'); - var orientation = new Quaternion(); - if (rpyValue) { - var rpy = rpyValue.split(' '); - // Convert from RPY - var roll = parseFloat(rpy[0]); - var pitch = parseFloat(rpy[1]); - var yaw = parseFloat(rpy[2]); - var phi = roll / 2.0; - var the = pitch / 2.0; - var psi = yaw / 2.0; - var x = - Math.sin(phi) * Math.cos(the) * Math.cos(psi) - - Math.cos(phi) * Math.sin(the) * Math.sin(psi); - var y = - Math.cos(phi) * Math.sin(the) * Math.cos(psi) + - Math.sin(phi) * Math.cos(the) * Math.sin(psi); - var z = - Math.cos(phi) * Math.cos(the) * Math.sin(psi) - - Math.sin(phi) * Math.sin(the) * Math.cos(psi); - var w = - Math.cos(phi) * Math.cos(the) * Math.cos(psi) + - Math.sin(phi) * Math.sin(the) * Math.sin(psi); + constructor({ xml }: UrdfDefaultOptions) { + this.name = xml.getAttribute(UrdfAttrs.Name); - orientation = new Quaternion({ - x: x, - y: y, - z: z, - w: w - }); - orientation.normalize(); - } - this.origin = new Pose({ - position: position, - orientation: orientation - }); + // Origin + const origins = xml.getElementsByTagName(UrdfAttrs.Origin); + if (origins.length > 0) { + this.origin = parseUrdfOrigin(origins[0]); } // Geometry - var geoms = xml.getElementsByTagName('geometry'); + const geoms = xml.getElementsByTagName(UrdfAttrs.Geometry); if (geoms.length > 0) { - var geom = geoms[0]; - var shape = null; - // Check for the shape - for (var i = 0; i < geom.childNodes.length; i++) { - /** @type {Element} */ - // @ts-expect-error -- unknown why this doesn't work properly. - var node = geom.childNodes[i]; - if (node.nodeType === 1) { - shape = node; - break; - } - } - if (shape) { - // Check the type - var type = shape.nodeName; - if (type === 'sphere') { - this.geometry = new UrdfSphere({ - xml: shape - }); - } else if (type === 'box') { - this.geometry = new UrdfBox({ - xml: shape - }); - } else if (type === 'cylinder') { - this.geometry = new UrdfCylinder({ - xml: shape - }); - } else if (type === 'mesh') { - this.geometry = new UrdfMesh({ - xml: shape - }); - } else { - console.warn('Unknown geometry type ' + type); - } - } + this.geometry = parseUrdfGeometry(geoms[0]); } // Material - var materials = xml.getElementsByTagName('material'); + const materials = xml.getElementsByTagName(UrdfAttrs.Material); if (materials.length > 0) { this.material = new UrdfMaterial({ xml: materials[0] diff --git a/src/urdf/index.ts b/src/urdf/index.ts index 8fa6d56c1..9b7fc33c5 100644 --- a/src/urdf/index.ts +++ b/src/urdf/index.ts @@ -4,8 +4,9 @@ export { default as UrdfCylinder } from './UrdfCylinder.js'; export { default as UrdfLink } from './UrdfLink.js'; export { default as UrdfMaterial } from './UrdfMaterial.js'; export { default as UrdfMesh } from './UrdfMesh.js'; -export { default as UrdfModel } from './UrdfModel.js'; +export { default as UrdfModel, type UrdfModelOptions } from './UrdfModel.js'; export { default as UrdfSphere } from './UrdfSphere.js'; -export { default as UrdfVisual } from './UrdfVisual.js'; +export { default as UrdfVisual, type UrdfGeometryLike } from './UrdfVisual.js'; -export * from './UrdfTypes.js'; +export {UrdfAttrs, UrdfType, type UrdfDefaultOptions} from './UrdfTypes.js'; +export {isElement, parseUrdfOrigin} from './UrdfUtils.js'; diff --git a/vite.config.ts b/vite.config.ts index 17464c0ff..ad91b63b3 100644 --- a/vite.config.ts +++ b/vite.config.ts @@ -5,19 +5,19 @@ import checker from 'vite-plugin-checker'; export default defineConfig({ plugins: [ - // dts({ - // tsconfigPath: 'tsconfig.build.json', - // insertTypesEntry: true - // }), - // checker({ - // typescript: { - // tsconfigPath: './tsconfig.build.json', - // }, - // eslint: { - // lintCommand: 'eslint .', - // useFlatConfig: true - // } - // }) + dts({ + tsconfigPath: 'tsconfig.build.json', + insertTypesEntry: true + }), + checker({ + typescript: { + tsconfigPath: './tsconfig.build.json', + }, + eslint: { + lintCommand: 'eslint .', + useFlatConfig: true + } + }) ], build: { lib: {