Kinematics features (#169)

* Kinematic Features
Signed-off-by: Lobotuerk <jtlorente@ekumenlabs.com>

bullet/src/KinematicsFeatures.cc

@@ -0,0 +1,60 @@
+#include <ignition/common/Console.hh>
+#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();
+
+  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 &model = linkInfo->link;
+
+  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;
+
+  // 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;
+}
+
+}
+}
+}

bullet/src/KinematicsFeatures.hh

@@ -0,0 +1,28 @@
+#ifndef IGNITION_PHYSICS_BULLET_SRC_KINEMATICSFEATURES_HH_
+#define IGNITION_PHYSICS_BULLET_SRC_KINEMATICSFEATURES_HH_
+
+#include <ignition/physics/FrameSemantics.hh>
+#include <ignition/physics/FreeGroup.hh>
+
+#include "Base.hh"
+
+namespace ignition {
+namespace physics {
+namespace bullet {
+
+using KinematicsFeatureList = FeatureList<
+  LinkFrameSemantics
+>;
+
+class KinematicsFeatures :
+    public virtual Base,
+    public virtual Implements3d<KinematicsFeatureList>
+{
+  public: FrameData3d FrameDataRelativeToWorld(const FrameID &_id) const;
+};
+
+}
+}
+}
+
+#endif

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
 {};