2 ➡️ 3 (#290)

Signed-off-by: Louise Poubel <louise@openrobotics.org>

.github/CODEOWNERS

@@ -1,5 +1,4 @@
 # More info:
 # https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners
 
-*                     @mxgrey
-include/*             @scpeters
+*                     @azeey @mxgrey @scpeters

dartsim/src/Base.hh

@@ -66,6 +66,7 @@ struct LinkInfo
 struct JointInfo
 {
   dart::dynamics::JointPtr joint;
+  dart::dynamics::SimpleFramePtr frame;
 };
 
 struct ShapeInfo
@@ -304,6 +305,13 @@ class Base : public Implements3d<FeatureList<Feature>>
     this->joints.idToObject[id] = std::make_shared<JointInfo>();
     this->joints.idToObject[id]->joint = _joint;
     this->joints.objectToID[_joint] = id;
+    dart::dynamics::SimpleFramePtr jointFrame =
+        dart::dynamics::SimpleFrame::createShared(
+            _joint->getChildBodyNode(), _joint->getName() + "_frame",
+            _joint->getTransformFromChildBodyNode());
+
+    this->joints.idToObject[id]->frame = jointFrame;
+    this->frames[id] = this->joints.idToObject[id]->frame.get();
 
     return id;
   }

dartsim/src/KinematicsFeatures.cc

@@ -40,6 +40,16 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
   }
 
   const dart::dynamics::Frame *frame = SelectFrame(_id);
+  // A missing frame ID indicates that frame semantics is not properly
+  // implemented for the type of frame represented by the ID.
+  if (nullptr == frame)
+  {
+    ignerr << "The frame ID " << _id.ID()
+           << " was not found in the list of known frames. This should not be "
+              "possible! Please report this bug!\n";
+    assert(false);
+    return data;
+  }
 
   data.pose = frame->getWorldTransform();
   data.linearVelocity = frame->getLinearVelocity();
@@ -62,7 +72,13 @@ const dart::dynamics::Frame *KinematicsFeatures::SelectFrame(
     return model_it->second->model->getRootBodyNode();
   }
 
-  return this->frames.at(_id.ID());
+  auto framesIt = this->frames.find(_id.ID());
+  if (framesIt == this->frames.end())
+  {
+    return nullptr;
+  }
+
+  return framesIt->second;
 }
 
 }

dartsim/src/KinematicsFeatures.hh

@@ -30,6 +30,7 @@ namespace dartsim {
 struct KinematicsFeatureList : FeatureList<
   LinkFrameSemantics,
   ShapeFrameSemantics,
+  JointFrameSemantics,
   FreeGroupFrameSemantics
 > { };
 

dartsim/src/KinematicsFeatures_TEST.cc

@@ -0,0 +1,134 @@
+/*
+ * Copyright (C) 2021 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 3.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+ */
+
+#include <gtest/gtest.h>
+
+#include <iostream>
+
+#include <ignition/physics/FindFeatures.hh>
+#include <ignition/plugin/Loader.hh>
+#include <ignition/physics/RequestEngine.hh>
+
+#include <ignition/math/eigen3/Conversions.hh>
+
+// Features
+#include <ignition/physics/FrameSemantics.hh>
+#include <ignition/physics/ForwardStep.hh>
+#include <ignition/physics/GetEntities.hh>
+#include <ignition/physics/Joint.hh>
+#include <ignition/physics/sdf/ConstructModel.hh>
+#include <ignition/physics/sdf/ConstructWorld.hh>
+
+#include <limits>
+#include <sdf/Model.hh>
+#include <sdf/Root.hh>
+#include <sdf/World.hh>
+
+#include "test/Utils.hh"
+
+using namespace ignition;
+
+using TestFeatureList = ignition::physics::FeatureList<
+  physics::ForwardStep,
+  physics::GetEntities,
+  physics::JointFrameSemantics,
+  physics::LinkFrameSemantics,
+  physics::sdf::ConstructSdfModel,
+  physics::sdf::ConstructSdfWorld
+>;
+
+using TestEnginePtr = physics::Engine3dPtr<TestFeatureList>;
+
+class KinematicsFeaturesFixture : public ::testing::Test
+{
+  protected: void SetUp() override
+  {
+    ignition::plugin::Loader loader;
+    loader.LoadLib(dartsim_plugin_LIB);
+
+    ignition::plugin::PluginPtr dartsim =
+        loader.Instantiate("ignition::physics::dartsim::Plugin");
+
+    this->engine =
+        ignition::physics::RequestEngine3d<TestFeatureList>::From(dartsim);
+    ASSERT_NE(nullptr, this->engine);
+  }
+  protected: TestEnginePtr engine;
+};
+
+// Test joint frame semantics
+TEST_F(KinematicsFeaturesFixture, JointFrameSemantics)
+{
+  sdf::Root root;
+  const sdf::Errors errors = root.Load(TEST_WORLD_DIR "string_pendulum.sdf");
+  ASSERT_TRUE(errors.empty()) << errors.front();
+
+  auto world = this->engine->ConstructWorld(*root.WorldByIndex(0));
+  ASSERT_NE(nullptr, world);
+
+  auto model = world->GetModel("pendulum");
+  ASSERT_NE(nullptr, model);
+  auto pivotJoint = model->GetJoint("pivot");
+  ASSERT_NE(nullptr, pivotJoint);
+  auto childLink = model->GetLink("bob");
+  ASSERT_NE(nullptr, childLink);
+
+  physics::ForwardStep::Output output;
+  physics::ForwardStep::State state;
+  physics::ForwardStep::Input input;
+
+  for (std::size_t i = 0; i < 100; ++i)
+  {
+    world->Step(output, state, input);
+  }
+  // Pose of Child link (C) in Joint frame (J)
+  physics::Pose3d X_JC = physics::Pose3d::Identity();
+  X_JC.translate(physics::Vector3d(0, 0, -1));
+
+  // Notation: Using F_WJ for the frame data of frame J (joint) relative to
+  // frame W (world).
+  auto F_WJ = pivotJoint->FrameDataRelativeToWorld();
+  physics::FrameData3d F_WCexpected = F_WJ;
+
+  physics::Vector3d pendulumArmInWorld =
+      F_WJ.pose.rotation() * X_JC.translation();
+
+  F_WCexpected.pose = F_WJ.pose * X_JC;
+  // angular acceleration of the child link is the same as the joint, so we
+  // don't need to assign a new value.
+
+  // Note that the joint's linear velocity and linear acceleration are zero, so
+  // they are omitted here.
+  F_WCexpected.linearAcceleration =
+      F_WJ.angularAcceleration.cross(pendulumArmInWorld) +
+      F_WJ.angularVelocity.cross(
+          F_WJ.angularVelocity.cross(pendulumArmInWorld));
+
+  F_WCexpected.linearVelocity = F_WJ.angularVelocity.cross(pendulumArmInWorld);
+
+  auto childLinkFrameData = childLink->FrameDataRelativeToWorld();
+  EXPECT_TRUE(
+      physics::test::Equal(F_WCexpected, childLinkFrameData, 1e-6));
+}
+
+/////////////////////////////////////////////////
+int main(int argc, char *argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
+

dartsim/worlds/string_pendulum.sdf

@@ -0,0 +1,73 @@
+<?xml version="1.0" ?>
+<sdf version="1.7">
+  <world name="string_pendulum.sdf">
+    <model name="pendulum">
+      <pose>0 0 1   0 0 0</pose>
+
+      <joint name="j0" type="fixed">
+        <parent>world</parent>
+        <child>support</child>
+      </joint>
+
+      <link name="support">
+        <inertial>
+          <mass>0.001</mass>
+        </inertial>
+      </link>
+
+      <joint name="pivot" type="revolute">
+        <!-- Rotate pivot so simulation starts with pendulum swinging  -->
+        <pose relative_to="support">0 0 0   0.7 0 0</pose>
+        <parent>support</parent>
+        <child>bob</child>
+        <axis>
+          <xyz>1 0 0</xyz>
+        </axis>
+      </joint>
+
+      <link name="bob">
+        <pose relative_to="pivot">0 0 -1   0 0 0</pose>
+        <inertial>
+          <mass>6</mass>
+          <inertia>
+            <ixx>0.01</ixx>
+            <iyy>0.01</iyy>
+            <izz>0.01</izz>
+          </inertia>
+        </inertial>
+        <visual name="bob_vis">
+          <geometry>
+            <sphere>
+              <radius>0.1</radius>
+            </sphere>
+          </geometry>
+          <material>
+            <ambient>0.0 0.8 0.0 1</ambient>
+            <diffuse>0.0 0.8 0.0 1</diffuse>
+          </material>
+        </visual>
+        <visual name="string_vis">
+          <pose>0 0 0.5   0 0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.01</radius>
+              <length>1.0</length>
+            </cylinder>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.0 1</ambient>
+            <diffuse>0.8 0.8 0.0 1</diffuse>
+          </material>
+        </visual>
+        <collision name="bob_col">
+          <geometry>
+            <sphere>
+              <radius>1</radius>
+            </sphere>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+  </world>
+</sdf>
+