Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support for continuous joints and fixed joints in URDF #16

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Support for continuous joints and fixed joints in URDF #16

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
cac8b55
add support for fixed joints and continuous joints
Cfather Nov 14, 2024
5825803
simple fix
Cfather Nov 14, 2024
5ce301e
Merge branch 'ROAM-Lab-ND:main' into main
Cfather Nov 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions v3/dynamics/jcalc.m
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,9 @@
R = T(1:3,1:3);
p = T(1:3,4);
Xj = [R zeros(3); skew(p)*R R];
case 'fixed'
Xj = eye(6);
S = [0;0;0;0;0;0];
otherwise
error( 'unrecognised joint code ''%s''', code );
end
16 changes: 14 additions & 2 deletions v3/models/URDF_to_spatialv2_model.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,10 +1,14 @@
function [model, robot] = URDF_to_spatialv2_model(file, addRandomInertia)
function [model, robot] = URDF_to_spatialv2_model(file, addRandomInertia, removeFixedJoints)
robot = importrobot(file);
model = struct();
model.NB = robot.NumBodies;
if nargin == 1
addRandomInertia = 0;
end

if nargin < 3
removeFixedJoints = 1;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

👍

end

% Loop over bodies to populate spatial_v2 model structure
for i =1:robot.NumBodies
Expand Down Expand Up @@ -48,10 +52,12 @@
%% Parse Joint information

% Get joint type
if strcmp(joint.Type,'revolute')
if strcmp(joint.Type,'revolute') || strcmp(joint.Type,'continuous')
jtype = 'R';
elseif strcmp(joint.Type,'prismatic')
jtype = 'P';
elseif strcmp(joint.Type,'fixed')
jtype = 'fixed';
else
% TODO: Implemented 'fixed' connections
assert(1==0,'Only revolute and prismatic supported.')
Expand All @@ -70,6 +76,8 @@
axis = 'y-';
elseif all( joint.JointAxis == [0 0 -1])
axis = 'z-';
elseif strcmp(joint.Type,'fixed')
axis = '';
else
% TODO: support non-axially aligned joints by redefining joint
% frames
Expand Down Expand Up @@ -106,6 +114,10 @@
X_i_iplus = AdjointRepresentation( inv(T_iplus_i) );
model.I{i} = X_i_iplus'*I_i*X_i_iplus;
end

if removeFixedJoints
model = remove_fixed_joints(model);
end
end

% Find a string in a list of strings. Return 0 if not found.
Expand Down
45 changes: 45 additions & 0 deletions v3/models/remove_fixed_joints.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
function model = remove_fixed_joints(model)
% Update inertias and transforms
for i = 1:model.NB
if ~strcmp(model.jtype{i}, 'fixed')
continue;
end

parent = model.parent(i);
children = find(model.parent == i);

% Update the transforms of the children
for j = children
model.parent(j) = parent;
model.Xtree{j} = model.Xtree{j}*model.Xtree{i};
end

% Absorb the body into the parent body if it exists
if parent > 0
% Add inertia to parent
X = model.Xtree{i};
I = X'*model.I{i}*X;
model.I{parent} = model.I{parent} + I;
end
end

% Remove the joints
i = 1;
while i <= model.NB
if ~strcmp(model.jtype{i}, 'fixed')
i = i + 1;
else
model.NB = model.NB - 1;
model.parent(i) = [];
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Have you tried using the resulting model in any of the conventional algorithms? My suspicion is that leaving an "empty" body in the list will lead to trouble. For example, the empty XTree cannot be applied to anything, and the empty parent would cause the kinematics propagation to fail as well.

model.Xtree(i) = [];
model.I(i) = [];
model.jtype(i) = [];

for j = i:model.NB
if model.parent(j) >= i
model.parent(j) = model.parent(j) - 1;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This line seems to be assuming that you are removing the body from the list and decrementing the body labels of everyone else in the tree by one? However all the old model data (e.g., for I, Xtree, etc.) remains stored at the original index.

I might be mentally parsing this incorrectly -- if so, I think an example URDF will help a great deal to instill confidence.

end
end
end
end
end