From c90b9c0cb1869bd4157d3c15cc7c1a8269cac520 Mon Sep 17 00:00:00 2001 From: Lobotuerk Date: Tue, 24 Nov 2020 17:44:47 -0300 Subject: [PATCH 1/2] Kinematic Features Signed-off-by: Lobotuerk --- bullet/src/KinematicsFeatures.cc | 98 ++++++++++++++++++++++++++++++++ bullet/src/KinematicsFeatures.hh | 28 +++++++++ bullet/src/plugin.cc | 3 + 3 files changed, 129 insertions(+) create mode 100644 bullet/src/KinematicsFeatures.cc create mode 100644 bullet/src/KinematicsFeatures.hh diff --git a/bullet/src/KinematicsFeatures.cc b/bullet/src/KinematicsFeatures.cc new file mode 100644 index 000000000..767f33ef4 --- /dev/null +++ b/bullet/src/KinematicsFeatures.cc @@ -0,0 +1,98 @@ +#include +#include "KinematicsFeatures.hh" + +namespace ignition { +namespace physics { +namespace bullet { + +///////////////////////////////////////////////// +FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( + const FrameID &_id) const +{ + FrameData3d data; + + // The feature system should never send us the world ID. + if (_id.IsWorld()) + { + ignerr << "Given a FrameID belonging to the world. This should not be " + << "possible! Please report this bug!\n"; + assert(false); + return data; + } + + const auto linkID = _id.ID(); + const auto &linkInfo = this->links.at(linkID); + const auto modelIdentity = linkInfo->model; + // auto linkIndex = linkInfo->linkIndex; + const auto &modelInfo = this->models.at(modelIdentity); + const auto &model = modelInfo->model; + + const btVector3 pos = model->getCenterOfMassTransform().getOrigin(); + const btMatrix3x3 mat = model->getCenterOfMassTransform().getBasis(); + + auto eigenMat = convert(mat); + auto eigenVec = convert(pos); + + data.pose.linear() = eigenMat; + data.pose.translation() = eigenVec; + + // Get frame velocities + // Note: only support revolute/fixed joints + // btVector3 vel = btVector3(0, 0, 0); + // btVector3 omega = btVector3(0, 0, 0); + // btMatrix3x3 matChildToThis = btMatrix3x3().getIdentity(); + // btVector3 rThisToChild = btVector3(0, 0, 0); + + // TODO(lobotuerk) uncomment once we support joints + // while (linkIndex != -1) + // { + // const auto &link = model->getLink(linkIndex); + // btScalar jointVel; + // if (link.m_jointType == btMultibodyLink::eFixed) + // jointVel = 0; + // else if (link.m_jointType == btMultibodyLink::eRevolute) + // jointVel = model->getJointVel(linkIndex); + // else + // { + // // ignerr << "Unregistered joint type for frame velocity computation.\n"; + // return data; + // } + // + // // Get target link's velocities in current link frame + // omega += jointVel * link.getAxisTop(0); + // vel += jointVel * link.getAxisBottom(0) + omega.cross( + // matChildToThis * rThisToChild); + // + // // Express in parent frame + // const auto matThisToParent = btMatrix3x3( + // link.m_cachedRotParentToThis).inverse(); + // omega = matThisToParent * omega; + // vel = matThisToParent * vel; + // + // // Info for next computation + // matChildToThis = matThisToParent; + // rThisToChild = link.m_cachedRVector; + // + // // Move on to parent + // linkIndex = link.m_parent; + // } + + // Add base velocities + btVector3 omega = model->getAngularVelocity(); + btVector3 vel = model->getLinearVelocity(); + // Transform to world frame + // const auto matBaseToWorld = btMatrix3x3(model->getWorldToBaseRot()).inverse(); + // omega = matBaseToWorld * omega; + // vel = matBaseToWorld * vel; + + data.linearVelocity = convert(vel); + data.angularVelocity = convert(omega); + + // \todo(anyone) compute frame accelerations + + return data; +} + +} +} +} diff --git a/bullet/src/KinematicsFeatures.hh b/bullet/src/KinematicsFeatures.hh new file mode 100644 index 000000000..95a090290 --- /dev/null +++ b/bullet/src/KinematicsFeatures.hh @@ -0,0 +1,28 @@ +#ifndef IGNITION_PHYSICS_BULLET_SRC_KINEMATICSFEATURES_HH_ +#define IGNITION_PHYSICS_BULLET_SRC_KINEMATICSFEATURES_HH_ + +#include +#include + +#include "Base.hh" + +namespace ignition { +namespace physics { +namespace bullet { + +using KinematicsFeatureList = FeatureList< + LinkFrameSemantics +>; + +class KinematicsFeatures : + public virtual Base, + public virtual Implements3d +{ + public: FrameData3d FrameDataRelativeToWorld(const FrameID &_id) const; +}; + +} +} +} + +#endif diff --git a/bullet/src/plugin.cc b/bullet/src/plugin.cc index f0201a504..07a12f5db 100644 --- a/bullet/src/plugin.cc +++ b/bullet/src/plugin.cc @@ -24,6 +24,7 @@ #include "EntityManagementFeatures.hh" #include "SimulationFeatures.hh" #include "SDFFeatures.hh" +#include "KinematicsFeatures.hh" namespace ignition { namespace physics { @@ -32,6 +33,7 @@ namespace bullet { struct BulletFeatures : FeatureList < EntityManagementFeatureList, SimulationFeatureList, + KinematicsFeatureList, SDFFeatureList > { }; @@ -40,6 +42,7 @@ class Plugin : public virtual Base, public virtual EntityManagementFeatures, public virtual SimulationFeatures, + public virtual KinematicsFeatures, public virtual SDFFeatures {}; From 25225aad75fe5f2d0ece6a5632f9c9526ac3235b Mon Sep 17 00:00:00 2001 From: Lobotuerk Date: Wed, 25 Nov 2020 10:50:34 -0300 Subject: [PATCH 2/2] rebased Signed-off-by: Lobotuerk --- bullet/src/KinematicsFeatures.cc | 52 +++++--------------------------- 1 file changed, 7 insertions(+), 45 deletions(-) diff --git a/bullet/src/KinematicsFeatures.cc b/bullet/src/KinematicsFeatures.cc index 767f33ef4..79c127f0d 100644 --- a/bullet/src/KinematicsFeatures.cc +++ b/bullet/src/KinematicsFeatures.cc @@ -21,11 +21,14 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( } const auto linkID = _id.ID(); + + if (this->links.find(linkID) == this->links.end()) + { + ignerr << "Given a FrameID not belonging to a link.\n"; + return data; + } const auto &linkInfo = this->links.at(linkID); - const auto modelIdentity = linkInfo->model; - // auto linkIndex = linkInfo->linkIndex; - const auto &modelInfo = this->models.at(modelIdentity); - const auto &model = modelInfo->model; + const auto &model = linkInfo->link; const btVector3 pos = model->getCenterOfMassTransform().getOrigin(); const btMatrix3x3 mat = model->getCenterOfMassTransform().getBasis(); @@ -36,47 +39,6 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld( data.pose.linear() = eigenMat; data.pose.translation() = eigenVec; - // Get frame velocities - // Note: only support revolute/fixed joints - // btVector3 vel = btVector3(0, 0, 0); - // btVector3 omega = btVector3(0, 0, 0); - // btMatrix3x3 matChildToThis = btMatrix3x3().getIdentity(); - // btVector3 rThisToChild = btVector3(0, 0, 0); - - // TODO(lobotuerk) uncomment once we support joints - // while (linkIndex != -1) - // { - // const auto &link = model->getLink(linkIndex); - // btScalar jointVel; - // if (link.m_jointType == btMultibodyLink::eFixed) - // jointVel = 0; - // else if (link.m_jointType == btMultibodyLink::eRevolute) - // jointVel = model->getJointVel(linkIndex); - // else - // { - // // ignerr << "Unregistered joint type for frame velocity computation.\n"; - // return data; - // } - // - // // Get target link's velocities in current link frame - // omega += jointVel * link.getAxisTop(0); - // vel += jointVel * link.getAxisBottom(0) + omega.cross( - // matChildToThis * rThisToChild); - // - // // Express in parent frame - // const auto matThisToParent = btMatrix3x3( - // link.m_cachedRotParentToThis).inverse(); - // omega = matThisToParent * omega; - // vel = matThisToParent * vel; - // - // // Info for next computation - // matChildToThis = matThisToParent; - // rThisToChild = link.m_cachedRVector; - // - // // Move on to parent - // linkIndex = link.m_parent; - // } - // Add base velocities btVector3 omega = model->getAngularVelocity(); btVector3 vel = model->getLinearVelocity();