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

Jump to bottom

Relative jacobian fix #109

Merged
merged 2 commits into from
Nov 13, 2023
Merged

Relative jacobian fix #109

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4037a20
[RBDyn] Do not try to remove joints if both ends of a relative jacobi…
BenjaminNavarro Dec 8, 2020
37f6361
[RBDyn] fix possible incorrect joint path for relative jacobians
BenjaminNavarro Nov 3, 2023
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
47 changes: 34 additions & 13 deletions src/RBDyn/Jacobian.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,25 @@ namespace rbd
Jacobian::Jacobian() {}

Jacobian::Jacobian(const MultiBody & mb, const std::string & bodyName, const Eigen::Vector3d & point)
: Jacobian(mb, bodyName, mb.body(0).name(), point)
Copy link
Member

Choose a reason for hiding this comment

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

I think this should still work, the first while loop in the constructor will break on index = 0 so the index != refBodyIndex_ branch is not taken and we end up with the same code

Copy link
Contributor Author

Choose a reason for hiding this comment

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

I ended up with the two constructors because I couldn't find an easy way to make the base case (no ref body) works after the second fix as it would always discard the model's root joint. I could use something like an empty string or a special name for the ref body to know that I need to go all the way to the root joint but I thought that in the end it was simpler and less error prone like that.

Copy link
Member

Choose a reason for hiding this comment

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

Ah I see, I walked through the code while looking at the current version but with the modified version it will indeed skip the root body.

I could use something like an empty string or a special name for the ref body to know that I need to go all the way to the root joint but I thought that in the end it was simpler and less error prone like that.

I think we can go with if(refBodyIndex_ != 0 && index == refBodyIndex_) as a condition to break?

: jointsPath_(), point_(point), jac_(), jacDot_()
{
bodyIndex_ = mb.sBodyIndexByName(bodyName);
refBodyIndex_ = 0;

int index = bodyIndex_;

int dof = 0;
while(index != -1)
{
jointsPath_.insert(jointsPath_.begin(), index);
dof += mb.joint(index).dof();
jointsSign_.insert(jointsSign_.begin(), 1);

index = mb.parent(index);
}

jac_.resize(6, dof);
jacDot_.resize(6, dof);
}

Jacobian::Jacobian(const MultiBody & mb,
Expand All @@ -38,14 +55,15 @@ Jacobian::Jacobian(const MultiBody & mb,
int dof = 0;
while(index != -1)
{
jointsPath_.insert(jointsPath_.begin(), index);
dof += mb.joint(index).dof();
jointsSign_.insert(jointsSign_.begin(), 1);

if(index == refBodyIndex_)
{
break;
}

jointsPath_.insert(jointsPath_.begin(), index);
dof += mb.joint(index).dof();
jointsSign_.insert(jointsSign_.begin(), 1);

index = mb.parent(index);
}

Expand All @@ -65,16 +83,19 @@ Jacobian::Jacobian(const MultiBody & mb,
count++;
} while(std::find(jointsPath_.begin() + count, jointsPath_.end(), index) == jointsPath_.end());

// Delete common joints previously added
int commonIdx = count;
while(jointsPath_[static_cast<size_t>(++commonIdx)] != index)
if(index > 0)
{
// Get to the common node
// Delete joints between the common joint and the root
int commonIdx = count;
while(jointsPath_[static_cast<size_t>(++commonIdx)] != index)
{
// Get to the common node
}
dof -= std::accumulate(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1, 0,
[&](int dofC, int idx) { return dofC + mb.joint(idx).dof(); });
jointsPath_.erase(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1);
jointsSign_.erase(jointsSign_.begin() + count, jointsSign_.begin() + commonIdx + 1);
}
dof -= std::accumulate(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1, 0,
[&](int dofC, int idx) { return dofC + mb.joint(idx).dof(); });
jointsPath_.erase(jointsPath_.begin() + count, jointsPath_.begin() + commonIdx + 1);
jointsSign_.erase(jointsSign_.begin() + count, jointsSign_.begin() + commonIdx + 1);
}

jac_.resize(6, dof);
Expand Down
Loading