From 87dfd088ddf83d57fbe199089bc77f2b7c477c47 Mon Sep 17 00:00:00 2001 From: Simon Manschitz Date: Fri, 18 Jun 2021 10:15:06 +0200 Subject: [PATCH 1/2] fixed inconsistent joint order after calling connectURDF --- src/RcsCore/Rcs_URDFParser.c | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/src/RcsCore/Rcs_URDFParser.c b/src/RcsCore/Rcs_URDFParser.c index 25c83446..7bdc25fc 100644 --- a/src/RcsCore/Rcs_URDFParser.c +++ b/src/RcsCore/Rcs_URDFParser.c @@ -545,6 +545,30 @@ 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) + { + RLOG(0, "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 +1073,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); From eedde205516eebaa7cef6fbf3f1fc33ad64e5c7e Mon Sep 17 00:00:00 2001 From: Simon Manschitz Date: Fri, 18 Jun 2021 10:27:01 +0200 Subject: [PATCH 2/2] log message is only printed if joint order is changed --- src/RcsCore/Rcs_URDFParser.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/RcsCore/Rcs_URDFParser.c b/src/RcsCore/Rcs_URDFParser.c index 7bdc25fc..636b5345 100644 --- a/src/RcsCore/Rcs_URDFParser.c +++ b/src/RcsCore/Rcs_URDFParser.c @@ -560,11 +560,14 @@ static void makeJointOrderConsistent(RcsBody* bdy) RcsJoint* prevJnt = RcsBody_lastJointBeforeBody(parentBdy); RCSBODY_TRAVERSE_JOINTS(BODY) { - RLOG(0, "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; + 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; + } } } }