diff --git a/src/RcsCore/Rcs_URDFParser.c b/src/RcsCore/Rcs_URDFParser.c
index 25c83446..636b5345 100644
--- a/src/RcsCore/Rcs_URDFParser.c
+++ b/src/RcsCore/Rcs_URDFParser.c
@@ -545,6 +545,33 @@ RcsJoint* parseJointURDF(xmlNode* node)
   return jnt;
 }
 
+/*******************************************************************************
+ * Corrects the joint order.
+ * When executing the connectURDF function, previous joints do not necessarily
+ * exist (yet) which can mess up the joint order for the Jacobian backward traversal.
+ * Therefore, after calling connectURDF, this function must be called for making
+ * the joint order consistent.
+ ******************************************************************************/
+static void makeJointOrderConsistent(RcsBody* bdy)
+{
+    RCSBODY_TRAVERSE_BODIES(bdy)
+    {
+        RcsBody* parentBdy = BODY->parent;
+        RcsJoint* prevJnt = RcsBody_lastJointBeforeBody(parentBdy);
+        RCSBODY_TRAVERSE_JOINTS(BODY)
+        {
+            if(prevJnt != JNT->prev)
+            {
+                RLOG(5, "Joint \"%s\" previous joint was \"%s\", previous joint now \"%s\"",
+                     JNT->name, (JNT->prev == NULL) ? "NULL" : JNT->prev->name,
+                     (prevJnt == NULL) ? "NULL" : prevJnt->name);
+                JNT->prev = prevJnt;
+                JNT->next = NULL;
+            }
+        }
+    }
+}
+
 /*******************************************************************************
  * Connect bodies and joints.
  * URDF is a bit limited in the sense that there's only one actuated joint
@@ -1049,6 +1076,9 @@ RcsBody* RcsGraph_rootBodyFromURDFFile(const char* filename,
   RCHECK_MSG(root, "Couldn't find root link in URFD model - did you "
              "define a cyclic model?");
 
+  // Make the joint order consistent
+  makeJointOrderConsistent(root);
+
   // Clean up
   xmlFreeDoc(doc);
   RFREE(bdyVec);