From cac8b550c0b2450d873dc5dcd7651b43d5be86ee Mon Sep 17 00:00:00 2001
From: Cfather <jimzhang@umich.edu>
Date: Thu, 14 Nov 2024 13:31:43 -0500
Subject: [PATCH 1/2] add support for fixed joints and continuous joints

---
 v3/dynamics/jcalc.m                 |  3 ++
 v3/models/URDF_to_spatialv2_model.m | 16 ++++++++--
 v3/models/remove_fixed_joints.m     | 45 +++++++++++++++++++++++++++++
 3 files changed, 62 insertions(+), 2 deletions(-)
 create mode 100644 v3/models/remove_fixed_joints.m

diff --git a/v3/dynamics/jcalc.m b/v3/dynamics/jcalc.m
index 2616222..1fb8608 100644
--- a/v3/dynamics/jcalc.m
+++ b/v3/dynamics/jcalc.m
@@ -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
diff --git a/v3/models/URDF_to_spatialv2_model.m b/v3/models/URDF_to_spatialv2_model.m
index 6121ee1..bce6323 100644
--- a/v3/models/URDF_to_spatialv2_model.m
+++ b/v3/models/URDF_to_spatialv2_model.m
@@ -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;
+    end
     
     % Loop over bodies to populate spatial_v2 model structure
     for i =1:robot.NumBodies
@@ -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.')
@@ -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
@@ -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.
diff --git a/v3/models/remove_fixed_joints.m b/v3/models/remove_fixed_joints.m
new file mode 100644
index 0000000..41ca051
--- /dev/null
+++ b/v3/models/remove_fixed_joints.m
@@ -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) = [];
+        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;
+            end
+        end
+    end
+end
+end
\ No newline at end of file

From 5825803b8b68bcb22aa5087ca025e9e55ce09cab Mon Sep 17 00:00:00 2001
From: Cfather <jimzhang@umich.edu>
Date: Thu, 14 Nov 2024 13:33:51 -0500
Subject: [PATCH 2/2] simple fix

---
 v3/models/URDF_to_spatialv2_model.m | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/v3/models/URDF_to_spatialv2_model.m b/v3/models/URDF_to_spatialv2_model.m
index bce6323..445968c 100644
--- a/v3/models/URDF_to_spatialv2_model.m
+++ b/v3/models/URDF_to_spatialv2_model.m
@@ -115,7 +115,7 @@
         model.I{i} = X_i_iplus'*I_i*X_i_iplus;
     end
 
-    if removeFixedJoints:
+    if removeFixedJoints
         model = remove_fixed_joints(model);
     end
 end