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

[dart] Compute Inverse Dynamics for all skeletons to populate joint forces #143

Closed
wants to merge 3 commits into from
Closed

[dart] Compute Inverse Dynamics for all skeletons to populate joint forces #143

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
437aade
Compute Inverse Dynamics to populate joint forces
diegoferigo Oct 22, 2020
e5d6f57
Calculate Inverse Dynamics only on enabled skeletons
diegoferigo Nov 2, 2020
52280e6
Merge branch 'ign-physics2' into fix/get_force
chapulina Feb 6, 2021
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
2 changes: 2 additions & 0 deletions dartsim/src/Base.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -338,6 +338,7 @@ class Base : public Implements3d<FeatureList<Feature>>
this->links.RemoveEntity(bn);
}
this->models.RemoveEntity(skel);
this->skeletonsWithInverseDynamics.erase(skel);
world->removeSkeleton(skel);
}

Expand All @@ -347,6 +348,7 @@ class Base : public Implements3d<FeatureList<Feature>>
public: EntityStorage<JointInfoPtr, const DartJoint*> joints;
public: EntityStorage<ShapeInfoPtr, const DartShapeNode*> shapes;
public: std::unordered_map<std::size_t, const dart::dynamics::Frame*> frames;
public: std::set<dart::dynamics::SkeletonPtr> skeletonsWithInverseDynamics;
};

}
Expand Down
10 changes: 10 additions & 0 deletions dartsim/src/JointFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,16 @@ double JointFeatures::GetJointAcceleration(
double JointFeatures::GetJointForce(
const Identity &_id, const std::size_t _dof) const
{
// Enable the computation of Inverse Dynamics on this skeleton
const auto &joint = this->ReferenceInterface<JointInfo>(_id)->joint;
if (this->skeletonsWithInverseDynamics.find(joint->getSkeleton()) ==
this->skeletonsWithInverseDynamics.end())
{
auto* thisNonConst = const_cast<JointFeatures*>(this);
thisNonConst->skeletonsWithInverseDynamics.insert(joint->getSkeleton());
joint->getSkeleton()->computeInverseDynamics();
}

return this->ReferenceInterface<JointInfo>(_id)->joint->getForce(_dof);
}

Expand Down
8 changes: 8 additions & 0 deletions dartsim/src/SimulationFeatures.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,14 @@ void SimulationFeatures::WorldForwardStep(
// TODO(MXG): Parse input
world->step();
// TODO(MXG): Fill in output and state

for (auto &skeleton: this->skeletonsWithInverseDynamics)
{
skeleton->computeInverseDynamics(
/*_withExternalForces=*/true,
/*_withDampingForces=*/true,
/*_withSpringForces=*/true);
}
}

std::vector<SimulationFeatures::ContactInternal>
Expand Down