And RelativeWrench, rename to JointTransmittedWrench

Signed-off-by: Addisu Z. Taddese <[email protected]>
gazebosim · Sep 20, 2021 · e9c5b3d · e9c5b3d
1 parent 45b52a4
commit e9c5b3d
Show file tree

Hide file tree

Showing 10 changed files with 813 additions and 1 deletion.
diff --git a/dartsim/src/JointFeatures.cc b/dartsim/src/JointFeatures.cc
@@ -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;
+}
 }
 }
 }
diff --git a/dartsim/src/JointFeatures.hh b/dartsim/src/JointFeatures.hh
@@ -55,7 +55,8 @@ struct JointFeatureList : FeatureList<
  SetJointVelocityCommandFeature,
  SetJointPositionLimitsFeature,
  SetJointVelocityLimitsFeature,
- SetJointEffortLimitsFeature
+ SetJointEffortLimitsFeature,
+ GetJointTransmittedWrench
 > { };
 
 class JointFeatures :
@@ -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;
 };
 
 }