Skip to content

Commit

Permalink
Add GetJointTransmittedWrench feature (#283)
Browse files Browse the repository at this point in the history
Adds a new feature that exposes the transmitted forces on a joint, which are needed for creating a Force/Torque sensor

Signed-off-by: Addisu Z. Taddese <[email protected]>
Signed-off-by: Steve Peters <[email protected]>
Co-authored-by: Steve Peters <[email protected]>
  • Loading branch information
azeey and scpeters authored Sep 21, 2021
1 parent 45b52a4 commit 049871a
Show file tree
Hide file tree
Showing 10 changed files with 814 additions and 1 deletion.
27 changes: 27 additions & 0 deletions dartsim/src/JointFeatures.cc
Original file line number Diff line number Diff line change
Expand Up @@ -645,6 +645,33 @@ Identity JointFeatures::AttachPrismaticJoint(
return this->GenerateIdentity(jointID, this->joints.at(jointID));
}

/////////////////////////////////////////////////
Wrench3d JointFeatures::GetJointTransmittedWrenchInJointFrame(
const Identity &_id) const
{
auto &joint = this->ReferenceInterface<JointInfo>(_id)->joint;
auto *childBn = joint->getChildBodyNode();
if (nullptr == childBn)
{
ignerr
<< "Joint [" << joint->getName()
<< "] does not have a child link. Unable to get transmitted wrench.\n";
return {};
}

const Eigen::Vector6d transmittedWrenchInBody = childBn->getBodyForce();
// C - Child body frame
// J - Joint frame
// X_CJ - Pose of joint in child body frame
const Eigen::Isometry3d X_CJ = joint->getTransformFromChildBodyNode();

const Eigen::Vector6d transmittedWrenchInJoint =
dart::math::dAdT(X_CJ, transmittedWrenchInBody);
Wrench3d wrenchOut;
wrenchOut.torque = transmittedWrenchInJoint.head<3>();
wrenchOut.force = transmittedWrenchInJoint.tail<3>();
return wrenchOut;
}
}
}
}
7 changes: 6 additions & 1 deletion dartsim/src/JointFeatures.hh
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,8 @@ struct JointFeatureList : FeatureList<
SetJointVelocityCommandFeature,
SetJointPositionLimitsFeature,
SetJointVelocityLimitsFeature,
SetJointEffortLimitsFeature
SetJointEffortLimitsFeature,
GetJointTransmittedWrench
> { };

class JointFeatures :
Expand Down Expand Up @@ -198,6 +199,10 @@ class JointFeatures :
public: void SetJointMaxEffort(
const Identity &_id, const std::size_t _dof,
const double _value) override;

// ----- Transmitted wrench -----
public: Wrench3d GetJointTransmittedWrenchInJointFrame(
const Identity &_id) const override;
};

}
Expand Down
Loading

0 comments on commit 049871a

Please sign in to comment.