Description: Fix velocity and position integration for FreeJoint
 Critical fix for the simulation of wheeled vehicles in robotics.
 All the information in: https://github.com/dartsim/dart/issues/1433
Origin: https://github.com/dartsim/dart/pull/1437
Forwarded: https://github.com/dartsim/dart/pull/1437
Applied-Upstream: https://github.com/dartsim/dart/pull/1437
Last-Update: 2021-12-20
---
This patch header follows DEP-3: http://dep.debian.net/deps/dep3/
diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp
index 4e2bc4fa15f2..a063cf3a2ac6 100644
--- a/dart/dynamics/FreeJoint.cpp
+++ b/dart/dynamics/FreeJoint.cpp
@@ -589,12 +589,59 @@ bool FreeJoint::isCyclic(std::size_t _index) const
 //==============================================================================
 void FreeJoint::integratePositions(double _dt)
 {
-  const Eigen::Isometry3d Qnext
-      = getQ() * convertToTransform(getVelocitiesStatic() * _dt);
+  const Eigen::Isometry3d Qdiff
+      = convertToTransform(getVelocitiesStatic() * _dt);
+  const Eigen::Isometry3d Qnext = getQ() * Qdiff;
+  const Eigen::Isometry3d QdiffInv = Qdiff.inverse();
 
+  setVelocitiesStatic(math::AdR(QdiffInv, getVelocitiesStatic()));
+  setAccelerationsStatic(math::AdR(QdiffInv, getAccelerationsStatic()));
   setPositionsStatic(convertToPositions(Qnext));
 }
 
+//==============================================================================
+void FreeJoint::integrateVelocities(double _dt)
+{
+  // Integrating the acceleration gives us the new velocity of child body frame.
+  // But if there is any linear acceleration, the frame will be displaced. If we
+  // apply euler integration direcly on the spatial acceleration, it will
+  // produce the velocity of a point that is instantaneously coincident with the
+  // previous location of the child body frame. However, we want to compute the
+  // spatial velocity at the current location of the child body frame. To
+  // accomplish this, we first convert the linear portion of the spatial
+  // acceleration into classical linear acceleration and apply the integration.
+  Eigen::Vector6d accel = getAccelerationsStatic();
+  const Eigen::Vector6d& velBefore = getVelocitiesStatic();
+  accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>());
+  setVelocitiesStatic(math::integrateVelocity<math::SE3Space>(
+      getVelocitiesStatic(), accel, _dt));
+
+  // Since the velocity has been updated, we use the new velocity to recompute
+  // the spatial acceleration. This is needed to ensure that functions like
+  // BodyNode::getLinearAcceleration work properly.
+  const Eigen::Vector6d& velAfter = getVelocitiesStatic();
+  accel.tail<3>() -= velAfter.head<3>().cross(velAfter.tail<3>());
+  setAccelerationsStatic(accel);
+}
+
+//==============================================================================
+void FreeJoint::updateConstrainedTerms(double timeStep)
+{
+  const double invTimeStep = 1.0 / timeStep;
+
+  const Eigen::Vector6d& velBefore = getVelocitiesStatic();
+  Eigen::Vector6d accel = getAccelerationsStatic();
+  accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>());
+
+  setVelocitiesStatic(getVelocitiesStatic() + mVelocityChanges);
+
+  const Eigen::Vector6d& velAfter = getVelocitiesStatic();
+  accel.tail<3>() -= velAfter.head<3>().cross(velAfter.tail<3>());
+  setAccelerationsStatic(accel + mVelocityChanges * invTimeStep);
+  this->mAspectState.mForces.noalias() += mImpulses * invTimeStep;
+  // Note: As long as this is only called from BodyNode::updateConstrainedTerms
+}
+
 //==============================================================================
 void FreeJoint::updateDegreeOfFreedomNames()
 {
diff --git a/dart/dynamics/FreeJoint.hpp b/dart/dynamics/FreeJoint.hpp
index 4432728c303a..22c463063a05 100644
--- a/dart/dynamics/FreeJoint.hpp
+++ b/dart/dynamics/FreeJoint.hpp
@@ -330,6 +330,12 @@ class FreeJoint : public GenericJoint<math::SE3Space>
   // Documentation inherited
   void integratePositions(double _dt) override;
 
+  // Documentation inherited
+  void integrateVelocities(double _dt) override;
+
+  // Documentation inherited
+  void updateConstrainedTerms(double timeStep) override;
+
   // Documentation inherited
   void updateDegreeOfFreedomNames() override;
 
diff --git a/data/sdf/test/issue1193_revolute_test.sdf b/data/sdf/test/issue1193_revolute_test.sdf
new file mode 100644
index 000000000000..a6cc24d012fd
--- /dev/null
+++ b/data/sdf/test/issue1193_revolute_test.sdf
@@ -0,0 +1,62 @@
+<sdf version="1.5">
+  <world name='default'>
+    <gravity>0 0 0</gravity>
+    <physics type="dart">
+      <gravity>0.0 0.0 0</gravity>
+      <real_time_update_rate>1000.000000</real_time_update_rate>
+      <max_step_size>0.001000</max_step_size>
+    </physics>
+    <model name="M">
+      <pose>1 1 0 0 0 0</pose>
+      <link name="link1">
+        <pose>0 0 2 0 0 0</pose>
+        <inertial>
+          <mass>1.0</mass>
+          <inertia>
+            <ixx>1.0</ixx>
+            <iyy>1.0</iyy>
+            <izz>1.0</izz>
+            <ixy>0.0</ixy>
+            <ixz>0.0</ixz>
+            <iyz>0.0</iyz>
+          </inertia>
+        </inertial>
+        <visual name="v1">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+        <visual name='v2'>
+          <pose>0 0 -2 0 0 0</pose>
+          <geometry><sphere><radius>0.1</radius></sphere></geometry>
+        </visual>
+      </link>
+      <joint name="revJoint" type="revolute">
+        <parent>link1</parent>
+        <child>link2</child>
+        <axis>
+          <xyz>1 0 0</xyz>
+        </axis>
+      </joint>
+      <link name="link2">
+        <pose>0 0 -2 0 0 0</pose>
+        <inertial>
+          <mass>1.0</mass>
+          <inertia>
+            <ixx>1.0</ixx>
+            <iyy>1.0</iyy>
+            <izz>1.0</izz>
+            <ixy>0.0</ixy>
+            <ixz>0.0</ixz>
+            <iyz>0.0</iyz>
+          </inertia>
+        </inertial>
+        <visual name='v1'>
+          <geometry><box><size>1 1 1</size></box></geometry>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>
diff --git a/data/sdf/test/issue1193_revolute_with_offset_test.sdf b/data/sdf/test/issue1193_revolute_with_offset_test.sdf
new file mode 100644
index 000000000000..13f813dc3209
--- /dev/null
+++ b/data/sdf/test/issue1193_revolute_with_offset_test.sdf
@@ -0,0 +1,62 @@
+<sdf version="1.5">
+  <world name='default'>
+    <gravity>0 0 0</gravity>
+    <physics type="dart">
+      <gravity>0.0 0.0 0</gravity>
+      <real_time_update_rate>1000.000000</real_time_update_rate>
+      <max_step_size>0.001000</max_step_size>
+    </physics>
+    <model name="M">
+      <link name="link1">
+        <pose>0 0 2 0 0 0</pose>
+        <inertial>
+          <mass>1.0</mass>
+          <inertia>
+            <ixx>1.0</ixx>
+            <iyy>1.0</iyy>
+            <izz>1.0</izz>
+            <ixy>0.0</ixy>
+            <ixz>0.0</ixz>
+            <iyz>0.0</iyz>
+          </inertia>
+        </inertial>
+        <visual name="v1">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+        <visual name='v2'>
+          <pose>0 0 -2 0 0 0</pose>
+          <geometry><sphere><radius>0.1</radius></sphere></geometry>
+        </visual>
+      </link>
+      <joint name="revJoint" type="revolute">
+        <pose>0 0 2 0 0 0</pose>
+        <parent>link1</parent>
+        <child>link2</child>
+        <axis>
+          <xyz>1 0 0</xyz>
+        </axis>
+      </joint>
+      <link name="link2">
+        <pose>0 0 -2 0 0 0</pose>
+        <inertial>
+          <mass>1.0</mass>
+          <inertia>
+            <ixx>1.0</ixx>
+            <iyy>1.0</iyy>
+            <izz>1.0</izz>
+            <ixy>0.0</ixy>
+            <ixz>0.0</ixz>
+            <iyz>0.0</iyz>
+          </inertia>
+        </inertial>
+        <visual name='v1'>
+          <geometry><box><size>1 1 1</size></box></geometry>
+        </visual>
+      </link>
+    </model>
+  </world>
+</sdf>
diff --git a/unittests/comprehensive/test_Dynamics.cpp b/unittests/comprehensive/test_Dynamics.cpp
index 12baf6661718..f89f3fe82f64 100644
--- a/unittests/comprehensive/test_Dynamics.cpp
+++ b/unittests/comprehensive/test_Dynamics.cpp
@@ -1117,31 +1117,19 @@ void DynamicsTest::testFiniteDifferenceGeneralizedCoordinates(
           = skeleton->getVelocityDifferences(dq1FD, dq0FD) / timeStep;
       VectorXd ddqFD2 = skeleton->getVelocityDifferences(dq2, dq1) / timeStep;
 
-      EXPECT_TRUE(equals(dq0, dq0FD, TOLERANCE));
-      EXPECT_TRUE(equals(dq1, dq1FD, TOLERANCE));
-      EXPECT_TRUE(equals(ddq0, ddqFD1, TOLERANCE));
-      EXPECT_TRUE(equals(ddq0, ddqFD2, TOLERANCE));
+      EXPECT_TRUE(equals(dq0, dq0FD, TOLERANCE))
+          << "dq0  : " << dq0.transpose() << "\ndq0FD: " << dq0FD.transpose();
 
-      if (!equals(dq0FD, dq0, TOLERANCE))
-      {
-        std::cout << "dq0  : " << dq0.transpose() << std::endl;
-        std::cout << "dq0FD: " << dq0FD.transpose() << std::endl;
-      }
-      if (!equals(dq1, dq1FD, TOLERANCE))
-      {
-        std::cout << "dq1  : " << dq1.transpose() << std::endl;
-        std::cout << "dq1FD: " << dq1FD.transpose() << std::endl;
-      }
-      if (!equals(ddq0, ddqFD1, TOLERANCE))
-      {
-        std::cout << "ddq0  : " << ddq0.transpose() << std::endl;
-        std::cout << "ddqFD1: " << ddqFD1.transpose() << std::endl;
-      }
-      if (!equals(ddq0, ddqFD2, TOLERANCE))
-      {
-        std::cout << "ddq0  : " << ddq0.transpose() << std::endl;
-        std::cout << "ddqFD2: " << ddqFD2.transpose() << std::endl;
-      }
+      EXPECT_TRUE(equals(dq1, dq1FD, TOLERANCE))
+          << "dq1  : " << dq1.transpose() << "\ndq1FD: " << dq1FD.transpose();
+
+      EXPECT_TRUE(equals(ddq0, ddqFD1, 10 * TOLERANCE))
+          << "ddq0  : " << ddq0.transpose()
+          << "\nddqFD1: " << ddqFD1.transpose();
+
+      EXPECT_TRUE(equals(ddq0, ddqFD2, 10 * TOLERANCE))
+          << "ddq0  : " << ddq0.transpose()
+          << "\nddqFD2: " << ddqFD2.transpose() << std::endl;
     }
   }
 }
@@ -2605,3 +2593,70 @@ TEST_F(DynamicsTest, HybridDynamics)
     EXPECT_NEAR(command(i, 4), output(i, 4), tol);
   }
 }
+
+//==============================================================================
+TEST_F(DynamicsTest, OffsetCOM)
+{
+  WorldPtr world = World::create();
+  ASSERT_TRUE(world != nullptr);
+  world->setGravity(Eigen::Vector3d(0.0, 0.0, 0.0));
+  const double dt = 0.001;
+  world->setTimeStep(dt);
+
+  SkeletonPtr boxSkel = createBox(Vector3d(1.0, 1.0, 1.0));
+  world->addSkeleton(boxSkel);
+  BodyNode* box = boxSkel->getBodyNode(0);
+  dynamics::Inertia inertia;
+  inertia.setMass(60);
+  inertia.setMoment(
+      box->getShapeNode(0)->getShape()->computeInertia(inertia.getMass()));
+  inertia.setLocalCOM({0, 500, 0});
+  box->setInertia(inertia);
+  Eigen::Isometry3d boxInitialPose = box->getWorldTransform();
+  Eigen::Vector3d torque(0, 0, 100);
+  box->addExtTorque(torque, false);
+
+  {
+    Vector3d comVel = box->getCOMLinearVelocity();
+    EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel))
+        << "comVel: " << comVel.transpose();
+  }
+  {
+    Vector3d linVel = box->getLinearVelocity();
+    EXPECT_TRUE(equals(Vector3d(0, 0, 0), linVel))
+        << "linVel: " << linVel.transpose();
+  }
+
+  world->step();
+  // Velocity at COM should be zero since the object is just rotating.
+  {
+    Vector3d comVel = box->getCOMLinearVelocity();
+    EXPECT_TRUE(equals(Vector3d(0, 0, 0), comVel, 1e-4))
+        << "comVel: " << comVel.transpose();
+  }
+  {
+    Vector3d angVel = box->getAngularVelocity();
+    // We can compute expectedAngAccel like so because the initial angular
+    // velocity is zero.
+    Vector3d expectedAngAccel = inertia.getMoment().inverse() * torque;
+    Vector3d expectedAngVel = expectedAngAccel * dt;
+    EXPECT_TRUE(equals(expectedAngVel, angVel))
+        << "angVel: " << angVel.transpose();
+    Vector3d linVel = box->getLinearVelocity();
+    Vector3d expLinVel
+        = angVel.cross(boxInitialPose.linear() * -box->getLocalCOM());
+    EXPECT_TRUE(equals(expLinVel, linVel))
+        << "Expected: " << expLinVel.transpose()
+        << "\nActual: " << linVel.transpose();
+
+    Vector3d angAccel = box->getAngularAcceleration();
+    EXPECT_TRUE(equals(expectedAngAccel, angAccel))
+        << "angAccel: " << angAccel.transpose();
+    Vector3d linAccel = box->getLinearAcceleration();
+    Vector3d expLinAccel
+        = expectedAngAccel.cross(boxInitialPose.linear() * -box->getLocalCOM());
+    EXPECT_TRUE(equals(expLinAccel, linAccel))
+        << "Expected: " << expLinAccel.transpose()
+        << "\nActual: " << linAccel.transpose();
+  }
+}
diff --git a/unittests/regression/CMakeLists.txt b/unittests/regression/CMakeLists.txt
index ace85bafc91c..bcbc62e12113 100644
--- a/unittests/regression/CMakeLists.txt
+++ b/unittests/regression/CMakeLists.txt
@@ -43,3 +43,8 @@ if(TARGET dart-collision-bullet AND TARGET dart-collision-ode)
     dart-utils)
 
 endif()
+
+if(TARGET dart-utils)
+  dart_add_test("regression" test_Issue1193)
+  target_link_libraries(test_Issue1193 dart-utils)
+endif()
diff --git a/unittests/regression/test_Issue1193.cpp b/unittests/regression/test_Issue1193.cpp
new file mode 100644
index 000000000000..42e7f33d5d4c
--- /dev/null
+++ b/unittests/regression/test_Issue1193.cpp
@@ -0,0 +1,321 @@
+/*
+ * Copyright (c) 2011-2021, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/master/LICENSE
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <gtest/gtest.h>
+
+#include "dart/dart.hpp"
+#include "dart/utils/sdf/SdfParser.hpp"
+
+#include "TestHelpers.hpp"
+
+using namespace dart::math;
+using namespace dart::collision;
+using namespace dart::dynamics;
+using namespace dart::simulation;
+using namespace dart::utils;
+
+//==============================================================================
+TEST(Issue1193, AngularVelAdd)
+{
+  WorldPtr world = World::create();
+  EXPECT_TRUE(world != nullptr);
+  world->setGravity(Eigen::Vector3d(0.0, -10.0, 0.0));
+  const double dt = 0.001;
+  world->setTimeStep(dt);
+
+  SkeletonPtr sphereSkel = createSphere(0.05, Vector3d(0.0, 1.0, 0.0));
+  BodyNode* sphere = sphereSkel->getBodyNode(0);
+  Joint* sphereJoint = sphere->getParentJoint();
+  sphereJoint->setVelocity(0, 10.0); // ang_x -> Affect lz and ly
+  sphereJoint->setVelocity(1, 10.0); // ang_y -> No effect
+  sphereJoint->setVelocity(2, 10.0); // ang_z -> Affect lx and ly
+  world->addSkeleton(sphereSkel);
+
+  Eigen::Vector3d linearVelBefore = sphere->getLinearVelocity();
+  EXPECT_EQ(Vector3d::Zero(), linearVelBefore);
+
+  int maxSteps = 500;
+  for (int i = 0; i < maxSteps; i++)
+  {
+    world->step();
+  }
+
+  Vector3d linearVelAfter = sphere->getLinearVelocity();
+  double lx = linearVelAfter[0];
+  double ly = linearVelAfter[1];
+  double lz = linearVelAfter[2];
+
+  EXPECT_NEAR(0.0, lx, 1e-8);
+  EXPECT_NEAR(maxSteps * world->getGravity().y() * dt, ly, 1e-8);
+  EXPECT_NEAR(0.0, lz, 1e-8);
+}
+
+const double tol = 1e-5;
+const int g_iters = 100000;
+
+Eigen::Vector3d computeWorldAngularMomentum(const SkeletonPtr skel)
+{
+  Eigen::Vector3d angMomentum = Eigen::Vector3d::Zero();
+  for (auto* bn : skel->getBodyNodes())
+  {
+    angMomentum += dart::math::dAdInvT(
+                       bn->getWorldTransform(),
+                       bn->getSpatialInertia() * bn->getSpatialVelocity())
+                       .head<3>();
+  }
+  return angMomentum;
+}
+
+//==============================================================================
+TEST(Issue1193, SingleBody)
+{
+  WorldPtr world = World::create();
+  const double dt = 0.001;
+  world->setTimeStep(dt);
+  world->setGravity(Vector3d::Zero());
+  SkeletonPtr skel = createBox({1, 1, 1});
+  world->addSkeleton(skel);
+  auto rootBn = skel->getRootBodyNode();
+
+  Eigen::Vector3d initPosition = rootBn->getWorldTransform().translation();
+  Vector6d vels = compose({0, 25, 0}, {0, 0, -100});
+
+  rootBn->getParentJoint()->setVelocities(vels);
+
+  rootBn->addExtTorque({0, 50, 0});
+  for (int i = 0; i < g_iters; ++i)
+  {
+    world->step();
+  }
+  Eigen::Vector3d positionDiff
+      = rootBn->getWorldTransform().translation() - initPosition;
+  EXPECT_NEAR(0.0, positionDiff.x(), tol);
+  EXPECT_NEAR(0.0, positionDiff.y(), tol);
+  EXPECT_NEAR(g_iters * dt * vels[5], positionDiff.z(), tol);
+}
+
+//==============================================================================
+TEST(Issue1193, SingleBodyWithOffDiagonalMoi)
+{
+  WorldPtr world = World::create();
+  const double dt = 0.001;
+  world->setTimeStep(dt);
+  world->setGravity(Vector3d::Zero());
+  SkeletonPtr skel = createBox({1, 4, 9});
+  world->addSkeleton(skel);
+  auto rootBn = skel->getRootBodyNode();
+  rootBn->setMomentOfInertia(1.1, 1.1, 0.7, 0.1, 0, 0);
+  Eigen::Vector3d initPosition = rootBn->getWorldTransform().translation();
+
+  // Setting this to compose({0, 25, 0}, {0, 0, -100}) causes the test to fail.
+  Vector6d vels = compose({0, 2.5, 0}, {0, 0, -10.0});
+
+  rootBn->getParentJoint()->setVelocities(vels);
+
+  rootBn->addExtTorque({0, 50, 0});
+  //
+  for (int i = 0; i < g_iters; ++i)
+  {
+    world->step();
+  }
+  Eigen::Vector3d positionDiff
+      = rootBn->getWorldTransform().translation() - initPosition;
+  EXPECT_NEAR(0.0, positionDiff.x(), tol);
+  EXPECT_NEAR(0.0, positionDiff.y(), tol);
+  EXPECT_NEAR(g_iters * dt * vels[5], positionDiff.z(), tol);
+}
+
+//==============================================================================
+TEST(Issue1193, SingleBodyWithJointOffset)
+{
+  WorldPtr world = World::create();
+  const double dt = 0.001;
+  world->setTimeStep(dt);
+  world->setGravity(Vector3d::Zero());
+  SkeletonPtr skel = createBox({1, 1, 1});
+  world->addSkeleton(skel);
+  auto rootBn = skel->getRootBodyNode();
+  rootBn->setMomentOfInertia(1.1, 1.1, 0.7, 0.1, 0, 0);
+  Eigen::Vector3d initPosition = rootBn->getWorldTransform().translation();
+
+  Vector6d vels = compose({0, 2.5, 0}, {0, 0, -10});
+
+  auto* freeJoint = dynamic_cast<FreeJoint*>(rootBn->getParentJoint());
+  freeJoint->setVelocities(vels);
+
+  Eigen::Isometry3d jointPoseInParent = Eigen::Isometry3d::Identity();
+  jointPoseInParent.translate(Eigen::Vector3d(0.0, 4.0, 0));
+  freeJoint->setTransformFromParentBodyNode(jointPoseInParent);
+
+  rootBn->addExtTorque({0, 50, 0});
+  for (int i = 0; i < g_iters; ++i)
+  {
+    world->step();
+  }
+
+  Eigen::Vector3d positionDiff
+      = rootBn->getWorldTransform().translation() - initPosition;
+  EXPECT_NEAR(0.0, positionDiff.x(), tol);
+  EXPECT_NEAR(4.0, positionDiff.y(), tol);
+  EXPECT_NEAR(g_iters * dt * vels[5], positionDiff.z(), tol);
+}
+
+//==============================================================================
+TEST(Issue1193, SingleBodyWithCOMOffset)
+{
+  WorldPtr world = World::create();
+  const double dt = 0.001;
+  world->setTimeStep(dt);
+  world->setGravity(Vector3d::Zero());
+  SkeletonPtr skel = createBox({1, 1, 1});
+  world->addSkeleton(skel);
+  auto rootBn = skel->getRootBodyNode();
+  rootBn->setMomentOfInertia(1.0 / 6, 1.0 / 6.0, 1.0 / 6.0, 0, 0, 0);
+  rootBn->setLocalCOM({1, 5, 8});
+  const Eigen::Vector3d extForce{10, 0, 0};
+  rootBn->addExtForce(extForce);
+  world->step();
+  auto comLinearVel = rootBn->getCOMLinearVelocity();
+  // TODO (azeey) Improve FreeJoint integration so that a higher tolerance is
+  // not necessary here.
+  EXPECT_NEAR(extForce.x() * dt, comLinearVel.x(), tol * 1e2);
+}
+
+//==============================================================================
+TEST(Issue1193, WithFixedJoint)
+{
+  WorldPtr world = World::create();
+  const double dt = 0.001;
+  world->setTimeStep(dt);
+  world->setGravity(Vector3d::Zero());
+  SkeletonPtr skel = createBox({1, 1, 1}, {0, 0, 2});
+  world->addSkeleton(skel);
+  auto rootBn = skel->getRootBodyNode();
+
+  Eigen::Isometry3d comRelPose;
+  comRelPose = Eigen::Translation3d(0, 0, -2);
+  auto comFrame = SimpleFrame::createShared(rootBn, "CombinedCOM", comRelPose);
+  Eigen::Vector3d initComPosition = comFrame->getWorldTransform().translation();
+
+  GenericJoint<R1Space>::Properties joint2Prop(std::string("joint2"));
+  BodyNode::Properties link2Prop(
+      BodyNode::AspectProperties(std::string("link2")));
+  link2Prop.mInertia.setMass(1.0);
+
+  auto pair = rootBn->createChildJointAndBodyNodePair<WeldJoint>(
+      WeldJoint::Properties(joint2Prop), link2Prop);
+  auto* joint = pair.first;
+
+  Eigen::Isometry3d jointPoseInParent = Eigen::Isometry3d::Identity();
+  jointPoseInParent.translate(Eigen::Vector3d(0.0, 0.0, -4));
+  joint->setTransformFromParentBodyNode(jointPoseInParent);
+
+  // TODO (azeey) Improve FreeJoint integration so we can test with larger
+  // forces. Currently, increasing these forces much more causes the test to
+  // fail.
+  rootBn->setExtTorque({0, 2500, 0});
+  rootBn->setExtForce({0, 0, -1000});
+
+  for (int i = 0; i < g_iters; ++i)
+  {
+    world->step();
+  }
+  Eigen::Vector3d positionDiff
+      = comFrame->getWorldTransform().translation() - initComPosition;
+  EXPECT_NEAR(0.0, positionDiff.x(), tol);
+  EXPECT_NEAR(0.0, positionDiff.y(), tol);
+}
+
+//==============================================================================
+TEST(Issue1193, WithRevoluteJoint)
+{
+  auto world = SdfParser::readWorld(
+      "dart://sample/sdf/test/issue1193_revolute_test.sdf");
+  ASSERT_TRUE(world != nullptr);
+  const double dt = 0.001;
+  world->setTimeStep(dt);
+
+  SkeletonPtr skel = world->getSkeleton(0);
+  auto rootBn = skel->getRootBodyNode();
+
+  Eigen::Isometry3d comRelPose;
+  comRelPose = Eigen::Translation3d(0, 0, -2);
+  auto comFrame = SimpleFrame::createShared(rootBn, "CombinedCOM", comRelPose);
+  Eigen::Vector3d initComPosition = comFrame->getWorldTransform().translation();
+
+  auto* joint = skel->getJoint("revJoint");
+  ASSERT_NE(nullptr, joint);
+
+  for (int i = 0; i < g_iters; ++i)
+  {
+    joint->setCommand(0, 0.1);
+    world->step();
+  }
+
+  Eigen::Vector3d positionDiff
+      = comFrame->getWorldTransform().translation() - initComPosition;
+  EXPECT_NEAR(0.0, positionDiff.x(), tol * 5e2);
+  EXPECT_NEAR(0.0, positionDiff.y(), tol * 5e2);
+}
+
+//==============================================================================
+TEST(Issue1193, ConservationOfMomentumWithRevoluteJointWithOffset)
+{
+  auto world = SdfParser::readWorld(
+      "dart://sample/sdf/test/issue1193_revolute_with_offset_test.sdf");
+  ASSERT_TRUE(world != nullptr);
+  const double dt = 0.0001;
+  world->setTimeStep(dt);
+  world->setGravity(Vector3d::Zero());
+
+  SkeletonPtr skel = world->getSkeleton(0);
+  auto link1 = skel->getBodyNode("link1");
+
+  auto* joint = skel->getJoint("revJoint");
+  ASSERT_NE(nullptr, joint);
+
+  link1->getParentJoint()->setVelocities(compose({0, 0.25, 0}, {0, 0, -0.1}));
+  world->step();
+  Eigen::Vector3d maxAngMomentumChange = Eigen::Vector3d::Zero();
+  Eigen::Vector3d h0 = computeWorldAngularMomentum(skel);
+  for (int i = 1; i < g_iters; ++i)
+  {
+    joint->setCommand(0, 0.1);
+    world->step();
+
+    Eigen::Vector3d hNext = computeWorldAngularMomentum(skel);
+    maxAngMomentumChange
+        = (hNext - h0).cwiseAbs().cwiseMax(maxAngMomentumChange);
+  }
+
+  EXPECT_NEAR(0.0, maxAngMomentumChange.norm(), tol * 10);
+}