Skip to content

Commit f10f254

Browse files
YahiewiYahiewi
committed
Add modify joint feature
1 parent db6ab29 commit f10f254

File tree

3 files changed

+320
-20
lines changed

3 files changed

+320
-20
lines changed

‎src/controls.ts‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -448,10 +448,15 @@ export class URDFControls extends GUI {
448448
*
449449
* @returns - The controls to trigger callbacks when editor settings change
450450
*/
451-
createEditorControls(addJointCallback: () => void, linkNames: string[] = []) {
451+
createEditorControls(
452+
addJointCallback: () => void,
453+
linkNames: string[] = [],
454+
jointNames: string[] = []
455+
) {
452456
if (this._isEmpty(this.controls.editor)) {
453457
const editorSettings = {
454458
'Cursor Link Selection': false,
459+
'Select Joint': 'New Joint',
455460
'Parent Link': 'none',
456461
'Child Link': 'none',
457462
'Joint Name': 'new_joint',
@@ -467,11 +472,15 @@ export class URDFControls extends GUI {
467472
};
468473

469474
const dropdownOptions = ['none', ...linkNames];
475+
const jointOptions = ['New Joint', ...jointNames];
470476

471477
this.controls.editor.mode = this._jointsEditorFolder.add(
472478
editorSettings,
473479
'Cursor Link Selection'
474480
);
481+
this.controls.editor.selectedJoint = this._jointsEditorFolder
482+
.add(editorSettings, 'Select Joint', jointOptions)
483+
.name('Select Joint');
475484
this.controls.editor.parent = this._jointsEditorFolder
476485
.add(editorSettings, 'Parent Link', dropdownOptions)
477486
.listen();

‎src/editor/urdf-editor.ts‎

Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -94,4 +94,148 @@ export class URDFEditor {
9494

9595
return this._serializer.serializeToString(urdf);
9696
}
97+
98+
/**
99+
* Modifies an existing joint in a URDF string.
100+
*
101+
* @param urdfString - The URDF string to modify.
102+
* @param jointName - The name of the joint to modify.
103+
* @param modifications - Partial joint properties to update.
104+
* @returns The modified URDF string.
105+
*/
106+
modifyJoint(
107+
urdfString: string,
108+
jointName: string,
109+
modifications: Partial<{
110+
type: string;
111+
parent: string;
112+
child: string;
113+
origin_xyz: string;
114+
origin_rpy: string;
115+
axis_xyz: string;
116+
lower: string;
117+
upper: string;
118+
effort: string;
119+
velocity: string;
120+
}>
121+
): string {
122+
const urdf = this._parser.parseFromString(urdfString, 'application/xml');
123+
const joints = urdf.getElementsByTagName('joint');
124+
125+
// Find the joint to modify
126+
let targetJoint: Element | null = null;
127+
for (let i = 0; i < joints.length; i++) {
128+
if (joints[i].getAttribute('name') === jointName) {
129+
targetJoint = joints[i];
130+
break;
131+
}
132+
}
133+
134+
if (!targetJoint) {
135+
throw new Error(`Joint "${jointName}" not found in URDF`);
136+
}
137+
138+
// Helper to create an indented text node
139+
const createIndent = (level: number) =>
140+
urdf.createTextNode(`\n${' '.repeat(level)}`);
141+
142+
// Helper to find or create a child element
143+
const findOrCreateElement = (parent: Element, tagName: string): Element => {
144+
const existing = parent.getElementsByTagName(tagName)[0];
145+
if (existing) {
146+
return existing;
147+
}
148+
149+
const newElement = urdf.createElement(tagName);
150+
parent.appendChild(createIndent(2));
151+
parent.appendChild(newElement);
152+
return newElement;
153+
};
154+
155+
// Update joint type if specified
156+
if (modifications.type !== undefined) {
157+
targetJoint.setAttribute('type', modifications.type);
158+
}
159+
160+
// Update parent link if specified
161+
if (modifications.parent !== undefined) {
162+
const parentElement = findOrCreateElement(targetJoint, 'parent');
163+
parentElement.setAttribute('link', modifications.parent);
164+
}
165+
166+
// Update child link if specified
167+
if (modifications.child !== undefined) {
168+
const childElement = findOrCreateElement(targetJoint, 'child');
169+
childElement.setAttribute('link', modifications.child);
170+
}
171+
172+
// Update origin if specified
173+
if (
174+
modifications.origin_xyz !== undefined ||
175+
modifications.origin_rpy !== undefined
176+
) {
177+
const originElement = findOrCreateElement(targetJoint, 'origin');
178+
if (modifications.origin_xyz !== undefined) {
179+
originElement.setAttribute('xyz', modifications.origin_xyz);
180+
}
181+
if (modifications.origin_rpy !== undefined) {
182+
originElement.setAttribute('rpy', modifications.origin_rpy);
183+
}
184+
}
185+
186+
// Get current or updated joint type for conditional elements
187+
const jointType =
188+
modifications.type || targetJoint.getAttribute('type') || '';
189+
190+
// Handle axis element based on joint type
191+
const axisElement = targetJoint.getElementsByTagName('axis')[0];
192+
const needsAxis = [
193+
'revolute',
194+
'continuous',
195+
'prismatic',
196+
'planar'
197+
].includes(jointType);
198+
199+
if (needsAxis) {
200+
if (modifications.axis_xyz !== undefined) {
201+
const axis = findOrCreateElement(targetJoint, 'axis');
202+
axis.setAttribute('xyz', modifications.axis_xyz);
203+
}
204+
} else if (axisElement) {
205+
// Remove axis if joint type doesn't need it
206+
targetJoint.removeChild(axisElement);
207+
}
208+
209+
// Handle limit element based on joint type
210+
const limitElement = targetJoint.getElementsByTagName('limit')[0];
211+
const needsLimits = ['revolute', 'prismatic'].includes(jointType);
212+
213+
if (needsLimits) {
214+
if (
215+
modifications.lower !== undefined ||
216+
modifications.upper !== undefined ||
217+
modifications.effort !== undefined ||
218+
modifications.velocity !== undefined
219+
) {
220+
const limit = findOrCreateElement(targetJoint, 'limit');
221+
if (modifications.lower !== undefined) {
222+
limit.setAttribute('lower', modifications.lower);
223+
}
224+
if (modifications.upper !== undefined) {
225+
limit.setAttribute('upper', modifications.upper);
226+
}
227+
if (modifications.effort !== undefined) {
228+
limit.setAttribute('effort', modifications.effort);
229+
}
230+
if (modifications.velocity !== undefined) {
231+
limit.setAttribute('velocity', modifications.velocity);
232+
}
233+
}
234+
} else if (limitElement) {
235+
// Remove limits if joint type doesn't need them
236+
targetJoint.removeChild(limitElement);
237+
}
238+
239+
return this._serializer.serializeToString(urdf);
240+
}
97241
}

‎src/layout.ts‎

Lines changed: 166 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -322,19 +322,46 @@ export class URDFLayout extends PanelLayout {
322322
) {
323323
const urdfString = this._context.model.toString();
324324
const editorControls = this._controlsPanel.controls.editor;
325-
const newUrdfString = this._editor.addJoint(urdfString, {
326-
name: editorControls.name.getValue(),
327-
type: editorControls.type.getValue(),
328-
parent: this._selectedLinks.parent.name,
329-
child: this._selectedLinks.child.name,
330-
origin_xyz: editorControls.origin_xyz.getValue(),
331-
origin_rpy: editorControls.origin_rpy.getValue(),
332-
axis_xyz: editorControls.axis_xyz.getValue(),
333-
lower: editorControls.lower.getValue(),
334-
upper: editorControls.upper.getValue(),
335-
effort: editorControls.effort.getValue(),
336-
velocity: editorControls.velocity.getValue()
337-
});
325+
const isModifying =
326+
editorControls.selectedJoint.getValue() !== 'New Joint';
327+
328+
let newUrdfString;
329+
330+
if (isModifying) {
331+
// Modify existing joint
332+
newUrdfString = this._editor.modifyJoint(
333+
urdfString,
334+
editorControls.selectedJoint.getValue(),
335+
{
336+
type: editorControls.type.getValue(),
337+
parent: this._selectedLinks.parent.name,
338+
child: this._selectedLinks.child.name,
339+
origin_xyz: editorControls.origin_xyz.getValue(),
340+
origin_rpy: editorControls.origin_rpy.getValue(),
341+
axis_xyz: editorControls.axis_xyz.getValue(),
342+
lower: editorControls.lower.getValue(),
343+
upper: editorControls.upper.getValue(),
344+
effort: editorControls.effort.getValue(),
345+
velocity: editorControls.velocity.getValue()
346+
}
347+
);
348+
} else {
349+
// Add new joint
350+
newUrdfString = this._editor.addJoint(urdfString, {
351+
name: editorControls.name.getValue(),
352+
type: editorControls.type.getValue(),
353+
parent: this._selectedLinks.parent.name,
354+
child: this._selectedLinks.child.name,
355+
origin_xyz: editorControls.origin_xyz.getValue(),
356+
origin_rpy: editorControls.origin_rpy.getValue(),
357+
axis_xyz: editorControls.axis_xyz.getValue(),
358+
lower: editorControls.lower.getValue(),
359+
upper: editorControls.upper.getValue(),
360+
effort: editorControls.effort.getValue(),
361+
velocity: editorControls.velocity.getValue()
362+
});
363+
}
364+
338365
this._context.model.fromString(newUrdfString);
339366

340367
// Update the robot model and refresh joint controls
@@ -344,23 +371,92 @@ export class URDFLayout extends PanelLayout {
344371
this._selectedLinks.child = { name: null, obj: null };
345372
editorControls.parent.setValue('none');
346373
editorControls.child.setValue('none');
374+
editorControls.selectedJoint.setValue('New Joint');
347375
this._interactionEditor.clearHighlights();
348376
}
349377
};
350378

351379
const linkNames = Object.keys(this._loader.robotModel.links);
380+
const jointNames = Object.keys(this._loader.robotModel.joints);
352381
const editorControls = this._controlsPanel.createEditorControls(
353382
addJointCallback,
354-
linkNames
383+
linkNames,
384+
jointNames
355385
);
356386

357-
editorControls.mode.onChange((enabled: boolean) => {
358-
this._interactionEditor.setLinkSelectorMode(enabled);
359-
if (!enabled) {
360-
this._selectedLinks.parent = { name: null, obj: null };
361-
this._selectedLinks.child = { name: null, obj: null };
387+
// Handle joint selection for modification
388+
editorControls.selectedJoint.onChange((selectedJoint: string) => {
389+
const isModifying = selectedJoint !== 'New Joint';
390+
391+
// Update button text
392+
editorControls.add.__li.querySelector('.property-name').textContent =
393+
isModifying ? 'Update Joint' : 'Add Joint';
394+
395+
if (isModifying) {
396+
const joint = this._loader.robotModel.joints[selectedJoint];
397+
const jointElement = this._getJointElementFromURDF(selectedJoint);
398+
399+
if (joint && jointElement) {
400+
editorControls.type.setValue(joint.jointType);
401+
402+
// Get parent and child links
403+
const parentLink =
404+
jointElement
405+
.getElementsByTagName('parent')[0]
406+
?.getAttribute('link') || 'none';
407+
const childLink =
408+
jointElement
409+
.getElementsByTagName('child')[0]
410+
?.getAttribute('link') || 'none';
411+
editorControls.parent.setValue(parentLink);
412+
editorControls.child.setValue(childLink);
413+
414+
// Get origin values
415+
const origin = jointElement.getElementsByTagName('origin')[0];
416+
editorControls.origin_xyz.setValue(
417+
origin?.getAttribute('xyz') || '0 0 0'
418+
);
419+
editorControls.origin_rpy.setValue(
420+
origin?.getAttribute('rpy') || '0 0 0'
421+
);
422+
423+
// Get axis values
424+
const axis = jointElement.getElementsByTagName('axis')[0];
425+
editorControls.axis_xyz.setValue(
426+
axis?.getAttribute('xyz') || '0 0 1'
427+
);
428+
429+
// Get limit, effort and velocity values
430+
const limit = jointElement.getElementsByTagName('limit')[0];
431+
editorControls.lower.setValue(limit?.getAttribute('lower') || '-1.0');
432+
editorControls.upper.setValue(limit?.getAttribute('upper') || '1.0');
433+
editorControls.effort.setValue(
434+
limit?.getAttribute('effort') || '0.0'
435+
);
436+
editorControls.velocity.setValue(
437+
limit?.getAttribute('velocity') || '0.0'
438+
);
439+
440+
// Update selected links for highlighting
441+
this._updateSelectedLinksFromJoint(parentLink, childLink);
442+
}
443+
} else {
444+
// Clear fields for new joint
445+
editorControls.name.setValue('new_joint');
446+
editorControls.type.setValue('revolute');
362447
editorControls.parent.setValue('none');
363448
editorControls.child.setValue('none');
449+
editorControls.origin_xyz.setValue('0 0 0');
450+
editorControls.origin_rpy.setValue('0 0 0');
451+
editorControls.axis_xyz.setValue('0 0 1');
452+
editorControls.lower.setValue('-1.0');
453+
editorControls.upper.setValue('1.0');
454+
editorControls.effort.setValue('0.0');
455+
editorControls.velocity.setValue('0.0');
456+
457+
this._selectedLinks.parent = { name: null, obj: null };
458+
this._selectedLinks.child = { name: null, obj: null };
459+
this._interactionEditor.clearHighlights();
364460
}
365461
});
366462

@@ -518,6 +614,57 @@ export class URDFLayout extends PanelLayout {
518614
});
519615
}
520616

617+
/**
618+
* Helper method to get joint element from URDF XML
619+
*/
620+
private _getJointElementFromURDF(jointName: string): Element | null {
621+
if (!this._context) {
622+
return null;
623+
}
624+
625+
const parser = new DOMParser();
626+
const urdf = parser.parseFromString(
627+
this._context.model.toString(),
628+
'application/xml'
629+
);
630+
const joints = urdf.getElementsByTagName('joint');
631+
632+
for (let i = 0; i < joints.length; i++) {
633+
if (joints[i].getAttribute('name') === jointName) {
634+
return joints[i];
635+
}
636+
}
637+
return null;
638+
}
639+
640+
/**
641+
* Helper method to update selected links based on joint parent/child
642+
*/
643+
private _updateSelectedLinksFromJoint(
644+
parentLink: string,
645+
childLink: string
646+
): void {
647+
if (parentLink !== 'none') {
648+
const link = this._loader.robotModel.links[parentLink];
649+
const linkObject = link?.children.find((c: any) => c.isURDFVisual)
650+
?.children[0];
651+
this._selectedLinks.parent = { name: parentLink, obj: linkObject };
652+
if (linkObject) {
653+
this._interactionEditor.highlightLink(linkObject, 'parent');
654+
}
655+
}
656+
657+
if (childLink !== 'none') {
658+
const link = this._loader.robotModel.links[childLink];
659+
const linkObject = link?.children.find((c: any) => c.isURDFVisual)
660+
?.children[0];
661+
this._selectedLinks.child = { name: childLink, obj: linkObject };
662+
if (linkObject) {
663+
this._interactionEditor.highlightLink(linkObject, 'child');
664+
}
665+
}
666+
}
667+
521668
/**
522669
* Refreshes the joint controls by clearing and recreating them
523670
*/

0 commit comments

Comments
 (0)