dartpy 7.0.0.dev0__cp313-cp313-manylinux_2_39_x86_64.whl

share/doc/dart/examples/hubo_puppet/main.cpp

@@ -0,0 +1,1464 @@
+/*
+ * Copyright (c) 2011-2025, The DART development contributors
+ * All rights reserved.
+ *
+ * The list of contributors can be found at:
+ *   https://github.com/dartsim/dart/blob/main/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 <dart/gui/osg/osg.hpp>
+
+#include <dart/utils/urdf/urdf.hpp>
+#include <dart/utils/utils.hpp>
+
+#include <dart/dart.hpp>
+
+using namespace dart::math;
+using namespace dart::dynamics;
+using namespace dart::simulation;
+
+class RelaxedPosture : public dart::optimizer::Function
+{
+public:
+  RelaxedPosture(
+      const Eigen::VectorXd& idealPosture,
+      const Eigen::VectorXd& lower,
+      const Eigen::VectorXd& upper,
+      const Eigen::VectorXd& weights,
+      bool enforceIdeal = false)
+    : enforceIdealPosture(enforceIdeal),
+      mIdeal(idealPosture),
+      mLower(lower),
+      mUpper(upper),
+      mWeights(weights)
+  {
+    int dofs = mIdeal.size();
+    if (mLower.size() != dofs || mWeights.size() != dofs
+        || mUpper.size() != dofs) {
+      dterr << "[RelaxedPose::RelaxedPose] Dimension mismatch:\n"
+            << "  ideal:   " << mIdeal.size() << "\n"
+            << "  lower:   " << mLower.size() << "\n"
+            << "  upper:   " << mUpper.size() << "\n"
+            << "  weights: " << mWeights.size() << "\n";
+    }
+    mResultVector.setZero(dofs);
+  }
+
+  double eval(const Eigen::VectorXd& _x) override
+  {
+    computeResultVector(_x);
+    return 0.5 * mResultVector.dot(mResultVector);
+  }
+
+  void evalGradient(
+      const Eigen::VectorXd& _x, Eigen::Map<Eigen::VectorXd> _grad) override
+  {
+    computeResultVector(_x);
+
+    _grad.setZero();
+    int smaller = std::min(mResultVector.size(), _grad.size());
+    for (int i = 0; i < smaller; ++i)
+      _grad[i] = mResultVector[i];
+  }
+
+  void computeResultVector(const Eigen::VectorXd& _x)
+  {
+    mResultVector.setZero();
+
+    if (enforceIdealPosture) {
+      for (int i = 0; i < _x.size(); ++i) {
+        if (mIdeal.size() <= i)
+          break;
+
+        mResultVector[i] = mWeights[i] * (_x[i] - mIdeal[i]);
+      }
+    } else {
+      for (int i = 0; i < _x.size(); ++i) {
+        if (mIdeal.size() <= i)
+          break;
+
+        if (_x[i] < mLower[i])
+          mResultVector[i] = mWeights[i] * (_x[i] - mLower[i]);
+        else if (mUpper[i] < _x[i])
+          mResultVector[i] = mWeights[i] * (_x[i] - mUpper[i]);
+      }
+    }
+  }
+
+  bool enforceIdealPosture;
+
+protected:
+  Eigen::VectorXd mResultVector;
+
+  Eigen::VectorXd mIdeal;
+
+  Eigen::VectorXd mLower;
+
+  Eigen::VectorXd mUpper;
+
+  Eigen::VectorXd mWeights;
+};
+
+static inline bool checkDist(Eigen::Vector3d& p, double a, double b)
+{
+  double d = p.norm();
+  double dmax = a + b;
+  double dmin = fabs(a - b);
+
+  if (d > dmax) {
+    p *= dmax / d;
+    return false;
+  } else if (d < dmin) {
+    p *= dmin / d;
+    return false;
+  } else {
+    return true;
+  }
+}
+
+static inline void clamp_sincos(double& sincos, bool& valid)
+{
+  if (sincos < -1) {
+    valid = false;
+    sincos = -1;
+  } else if (sincos > 1) {
+    valid = false;
+    sincos = 1;
+  }
+}
+
+static inline Eigen::Vector3d flipEuler3Axis(const Eigen::Vector3d& u)
+{
+  Eigen::Vector3d v;
+  v[0] = u[0] - constantsd::pi();
+  v[1] = constantsd::pi() - u[1];
+  v[2] = u[2] - constantsd::pi();
+  return v;
+}
+
+/// The HuboArmIK is based on the derivation of Hubo's arm IK by Matt Zucker.
+class HuboArmIK : public InverseKinematics::Analytical
+{
+public:
+  HuboArmIK(
+      InverseKinematics* _ik,
+      const std::string& baseLinkName,
+      const Analytical::Properties& properties = Analytical::Properties())
+    : Analytical(_ik, "HuboArmIK_" + baseLinkName, properties),
+      configured(false),
+      mBaseLinkName(baseLinkName)
+  {
+    // Do nothing
+  }
+
+  std::unique_ptr<GradientMethod> clone(
+      InverseKinematics* _newIK) const override
+  {
+    return std::make_unique<HuboArmIK>(
+        _newIK, mBaseLinkName, getAnalyticalProperties());
+  }
+
+  const std::vector<Solution>& computeSolutions(
+      const Eigen::Isometry3d& _desiredBodyTf) override
+  {
+    mSolutions.clear();
+    mSolutions.reserve(8);
+
+    if (!configured) {
+      configure();
+
+      if (!configured) {
+        dtwarn
+            << "[HuboArmIK::computeSolutions] This analytical IK was not able "
+            << "to configure properly, so it will not be able to compute "
+            << "solutions\n";
+        return mSolutions;
+      }
+    }
+
+    const BodyNodePtr& base = mBaseLink.lock();
+    if (nullptr == base) {
+      dterr << "[HuboArmIK::computeSolutions] Attempting to perform an IK on a "
+            << "limb that no longer exists [" << getMethodName() << "]!\n";
+      assert(false);
+      return mSolutions;
+    }
+
+    if (nullptr == mWristEnd) {
+      dterr << "[HuboArmIK::computeSolutions] Attempting to perform IK without "
+            << "a wrist!\n";
+      assert(false);
+      return mSolutions;
+    }
+
+    const std::size_t SP = 0;
+    const std::size_t SR = 1;
+    const std::size_t SY = 2;
+    const std::size_t EP = 3;
+    const std::size_t WY = 4;
+    const std::size_t WP = 5;
+
+    const SkeletonPtr& skel = base->getSkeleton();
+
+    Eigen::Isometry3d B
+        = base->getParentBodyNode()->getWorldTransform().inverse()
+          * _desiredBodyTf * mWristEnd->getTransform(mIK->getNode());
+
+    Eigen::Isometry3d shoulder_from_wrist = shoulderTf.inverse() * B;
+    Eigen::Vector3d p = shoulder_from_wrist.inverse().translation();
+
+    const double a2 = L5 * L5 + L4 * L4;
+    const double b2 = L3 * L3 + L4 * L4;
+    const double a = sqrt(a2);
+    const double b = sqrt(b2);
+
+    const double alpha = atan2(L5, L4);
+    const double beta = atan2(L3, L4);
+
+    bool startValid = checkDist(p, a, b);
+
+    double c2 = p.dot(p);
+    double x = p.x();
+    double y = p.y();
+    double z = p.z();
+
+    for (std::size_t i = 0; i < 8; ++i) {
+      const int flipEP = alterantives(i, 0);
+      const int incWY = alterantives(i, 1);
+      const int flipShoulder = alterantives(i, 2);
+
+      Eigen::Vector6d testQ;
+      bool isValid = startValid;
+
+      double cosGamma = (a2 + b2 - c2) / (2 * a * b);
+      clamp_sincos(cosGamma, isValid);
+
+      double gamma = flipEP * acos(cosGamma);
+      double theta3 = alpha + beta + gamma - 2 * constantsd::pi();
+
+      testQ(EP) = theta3;
+
+      double c3 = cos(theta3);
+      double s3 = sin(theta3);
+
+      double numer = -y;
+      double denom = (-L4 * c3 - L3 * s3 + L4);
+
+      double s2, theta2;
+
+      if (std::abs(denom) < zeroSize) {
+        isValid = false;
+        const double& prevWY = skel->getPosition(mDofs[WY]);
+        theta2 = incWY ? prevWY : constantsd::pi() - prevWY;
+        s2 = sin(theta2);
+      } else {
+        s2 = numer / denom;
+        clamp_sincos(s2, isValid);
+        theta2 = incWY ? constantsd::pi() - asin(s2) : asin(s2);
+      }
+
+      testQ(WY) = theta2;
+
+      double c2 = cos(theta2);
+
+      double r = L4 * c2 - L4 * c2 * c3 - L3 * s3 * c2;
+      double q = -L4 * s3 + L3 * c3 + L5;
+
+      double det = -(q * q + r * r);
+
+      if (std::abs(det) < zeroSize)
+        isValid = false;
+
+      double k = det < 0 ? -1 : 1;
+
+      double ks1 = k * (q * x - r * z);
+      double kc1 = k * (-r * x - q * z);
+
+      double theta1 = atan2(ks1, kc1);
+      testQ(WP) = theta1;
+
+      Eigen::Quaterniond Rlower = Eigen::Quaterniond(Eigen::AngleAxisd(
+                                      testQ(EP), Eigen::Vector3d::UnitY()))
+                                  * Eigen::Quaterniond(Eigen::AngleAxisd(
+                                      testQ(WY), Eigen::Vector3d::UnitZ()))
+                                  * Eigen::Quaterniond(Eigen::AngleAxisd(
+                                      testQ(WP), Eigen::Vector3d::UnitY()));
+
+      Eigen::Matrix3d Rupper = B.rotation() * Rlower.inverse().matrix();
+
+      Eigen::Vector3d euler = Rupper.eulerAngles(1, 0, 2);
+
+      if (flipShoulder)
+        euler = flipEuler3Axis(euler);
+
+      testQ(SP) = euler[0];
+      testQ(SR) = euler[1];
+      testQ(SY) = euler[2];
+
+      for (std::size_t j = 0; j < 6; ++j) {
+        testQ[j] = dart::math::wrapToPi(testQ[j]);
+        if (std::abs(testQ[j]) < zeroSize)
+          testQ[j] = 0.0;
+      }
+
+      int validity = isValid ? VALID : OUT_OF_REACH;
+      mSolutions.push_back(Solution(testQ, validity));
+    }
+
+    checkSolutionJointLimits();
+
+    return mSolutions;
+  }
+
+  const std::vector<std::size_t>& getDofs() const override
+  {
+    if (!configured)
+      configure();
+
+    return mDofs;
+  }
+
+  const double zeroSize = 1e-8;
+
+protected:
+  void configure() const
+  {
+    configured = false;
+
+    mBaseLink = mIK->getNode()->getSkeleton()->getBodyNode(mBaseLinkName);
+
+    BodyNode* base = mBaseLink.lock();
+    if (nullptr == base) {
+      dterr << "[HuboArmIK::configure] base link is a nullptr\n";
+      assert(false);
+      return;
+    }
+
+    const SkeletonPtr& skel = base->getSkeleton();
+    const BodyNodePtr& pelvis = skel->getBodyNode("Body_TSY");
+    if (nullptr == pelvis) {
+      dterr << "[HuboArmIK::configure] Could not find Hubo's pelvis "
+            << "(Body_TSY)\n";
+      assert(false);
+      return;
+    }
+
+    Eigen::Vector6d saved_q;
+
+    DegreeOfFreedom* dofs[6];
+    BodyNode* bn = base;
+    for (std::size_t i = 0; i < 6; ++i) {
+      Joint* joint = bn->getParentJoint();
+      if (joint->getNumDofs() != 1) {
+        dterr << "[HuboArmIK::configure] Invalid number of DOFs ("
+              << joint->getNumDofs() << ") in the Joint [" << joint->getName()
+              << "]\n";
+        assert(false);
+        return;
+      }
+
+      dofs[i] = joint->getDof(0);
+      saved_q[i] = dofs[i]->getPosition();
+      dofs[i]->setPosition(0.0);
+      bn = bn->getChildBodyNode(0);
+    }
+
+    BodyNode* elbow = dofs[3]->getChildBodyNode();
+    L3 = std::abs(
+        elbow->getTransform(dofs[2]->getParentBodyNode()).translation()[2]);
+    L4 = std::abs(
+        elbow->getTransform(dofs[3]->getParentBodyNode()).translation()[0]);
+
+    BodyNode* wrist = dofs[5]->getChildBodyNode();
+    Eigen::Isometry3d wrist_tf = wrist->getTransform(elbow);
+    L5 = std::abs(wrist_tf.translation()[2]);
+
+    shoulderTf = Eigen::Isometry3d::Identity();
+    shoulderTf.translate(
+        dofs[3]->getParentBodyNode()->getTransform(pelvis).translation()[0]
+        * Eigen::Vector3d::UnitX());
+    shoulderTf.translate(
+        dofs[2]->getParentBodyNode()->getTransform(pelvis).translation()[1]
+        * Eigen::Vector3d::UnitY());
+    shoulderTf.translate(
+        dofs[2]->getParentBodyNode()->getTransform(pelvis).translation()[2]
+        * Eigen::Vector3d::UnitZ());
+
+    mWristEnd = dofs[5]->getChildBodyNode();
+
+    alterantives << 1, 1, 1, 1, 1, 0, 1, 0, 1, 1, 0, 0, -1, 1, 1, -1, 1, 0, -1,
+        0, 1, -1, 0, 0;
+
+    for (std::size_t i = 0; i < 6; ++i) {
+      dofs[i]->setPosition(saved_q[i]);
+      mDofs.push_back(dofs[i]->getIndexInSkeleton());
+    }
+
+    configured = true;
+  }
+
+  mutable bool configured;
+
+  mutable Eigen::Isometry3d shoulderTf;
+  mutable Eigen::Isometry3d wristTfInv;
+  mutable Eigen::Isometry3d mNodeOffsetTfInv;
+  mutable double L3, L4, L5;
+
+  mutable Eigen::Matrix<int, 8, 3> alterantives;
+
+  mutable std::vector<std::size_t> mDofs;
+
+  std::string mBaseLinkName;
+  mutable WeakBodyNodePtr mBaseLink;
+
+  mutable JacobianNode* mWristEnd;
+};
+
+class HuboLegIK : public InverseKinematics::Analytical
+{
+public:
+  /// baseLink should be Body_LHY or Body_RHY
+  HuboLegIK(
+      InverseKinematics* _ik,
+      const std::string& baseLinkName,
+      const Analytical::Properties& properties = Analytical::Properties())
+    : Analytical(_ik, "HuboLegIK_" + baseLinkName, properties),
+      configured(false),
+      mBaseLinkName(baseLinkName)
+  {
+    // Do nothing
+  }
+
+  std::unique_ptr<GradientMethod> clone(
+      InverseKinematics* _newIK) const override
+  {
+    return std::make_unique<HuboLegIK>(
+        _newIK, mBaseLinkName, getAnalyticalProperties());
+  }
+
+  const std::vector<Solution>& computeSolutions(
+      const Eigen::Isometry3d& _desiredBodyTf) override
+  {
+    mSolutions.clear();
+    mSolutions.reserve(8);
+
+    if (!configured) {
+      configure();
+
+      if (!configured) {
+        dtwarn
+            << "[HuboLegIK::computeSolutions] This analytical IK was not able "
+            << "to configure properly, so it will not be able to compute "
+            << "solutions\n";
+        return mSolutions;
+      }
+    }
+
+    const BodyNodePtr& base = mBaseLink.lock();
+    if (nullptr == base) {
+      dterr << "[HuboLegIK::computeSolutions] Attempting to perform IK on a "
+            << "limb that no longer exists!\n";
+      assert(false);
+      return mSolutions;
+    }
+
+    double nx, ny, sx, sy, ax, ay, az, px, py, pz;
+    double q1, q2, q3, q4, q5, q6;
+    double S2, S4, S6;
+    double C2, C4, C5, C6;
+    double C45, psi, q345;
+    std::complex<double> radical;
+    std::complex<double> sqrt_radical;
+    Eigen::Isometry3d B, Binv;
+
+    Eigen::Vector6d testQ;
+
+    B = (base->getParentBodyNode()->getWorldTransform() * waist).inverse()
+        * _desiredBodyTf * footTfInv;
+    Binv = B.inverse();
+
+    nx = Binv(0, 0);
+    sx = Binv(0, 1);
+    ax = Binv(0, 2);
+    px = Binv(0, 3);
+    ny = Binv(1, 0);
+    sy = Binv(1, 1);
+    ay = Binv(1, 2);
+    py = Binv(1, 3);
+    az = Binv(2, 2);
+    pz = Binv(2, 3);
+
+    for (std::size_t i = 0; i < 8; ++i) {
+      bool isValid = true;
+
+      C4 = ((px + L6) * (px + L6) - L4 * L4 - L5 * L5 + py * py + pz * pz)
+           / (2 * L4 * L5);
+      radical = 1 - C4 * C4;
+      sqrt_radical = std::sqrt(radical);
+      if (sqrt_radical.imag() != 0)
+        isValid = false;
+      q4 = atan2(alternatives(i, 0) * sqrt_radical.real(), C4);
+
+      S4 = sin(q4);
+      psi = atan2(S4 * L4, C4 * L4 + L5);
+      radical = (px + L6) * (px + L6) + py * py;
+      sqrt_radical = std::sqrt(radical);
+      if (sqrt_radical.imag() != 0)
+        isValid = false;
+
+      q5 = dart::math::wrapToPi(
+          atan2(-pz, alternatives(i, 1) * sqrt_radical.real()) - psi);
+
+      q6 = atan2(py, -(px + L6));
+      C45 = cos(q4 + q5);
+      C5 = cos(q5);
+      if (C45 * L4 + C5 * L5 < 0)
+        q6 = dart::math::wrapToPi(q6 + constantsd::pi());
+
+      S6 = sin(q6);
+      C6 = cos(q6);
+
+      S2 = C6 * ay + S6 * ax;
+      radical = 1 - S2 * S2;
+      sqrt_radical = std::sqrt(radical);
+      if (sqrt_radical.imag() != 0)
+        isValid = false;
+      q2 = atan2(S2, alternatives(i, 2) * sqrt_radical.real());
+
+      q1 = atan2(C6 * sy + S6 * sx, C6 * ny + S6 * nx);
+      C2 = cos(q2);
+      if (C2 < 0)
+        q1 = dart::math::wrapToPi(q1 + constantsd::pi());
+
+      q345 = atan2(-az / C2, -(C6 * ax - S6 * ay) / C2);
+      q3 = dart::math::wrapToPi(q345 - q4 - q5);
+
+      testQ[0] = q1;
+      testQ[1] = q2;
+      testQ[2] = q3;
+      testQ[3] = q4;
+      testQ[4] = q5;
+      testQ[5] = q6;
+
+      for (int k = 0; k < testQ.size(); ++k)
+        if (fabs(testQ[k]) < zeroSize)
+          testQ[k] = 0;
+
+      int validity = isValid ? VALID : OUT_OF_REACH;
+      mSolutions.push_back(Solution(testQ, validity));
+    }
+
+    checkSolutionJointLimits();
+
+    return mSolutions;
+  }
+
+  const std::vector<std::size_t>& getDofs() const override
+  {
+    if (!configured)
+      configure();
+
+    return mDofs;
+  }
+
+  const double zeroSize = 1e-8;
+
+protected:
+  void configure() const
+  {
+    configured = false;
+
+    mBaseLink = mIK->getNode()->getSkeleton()->getBodyNode(mBaseLinkName);
+
+    BodyNode* base = mBaseLink.lock();
+    if (nullptr == base) {
+      dterr << "[HuboLegIK::configure] base link is a nullptr\n";
+      assert(false);
+      return;
+    }
+
+    const SkeletonPtr& skel = mIK->getNode()->getSkeleton();
+    BodyNode* pelvis = skel->getBodyNode("Body_TSY");
+    if (nullptr == pelvis) {
+      dterr << "[HuboLegIK::configure] Could not find Hubo's pelvis "
+            << "(Body_TSY)\n";
+      assert(false);
+      return;
+    }
+
+    Eigen::Vector6d saved_q;
+
+    DegreeOfFreedom* dofs[6];
+    BodyNode* bn = base;
+    for (std::size_t i = 0; i < 6; ++i) {
+      Joint* joint = bn->getParentJoint();
+      if (joint->getNumDofs() != 1) {
+        dterr << "[HuboLegIK::configure] Invalid number of DOFs ("
+              << joint->getNumDofs() << ") in the Joint [" << joint->getName()
+              << "]\n";
+        assert(false);
+        return;
+      }
+
+      dofs[i] = joint->getDof(0);
+      saved_q[i] = dofs[i]->getPosition();
+      dofs[i]->setPosition(0.0);
+
+      if (bn->getNumChildBodyNodes() > 0)
+        bn = bn->getChildBodyNode(0);
+    }
+
+    L4 = std::abs(
+        dofs[3]->getChildBodyNode()->getRelativeTransform().translation()[2]);
+
+    L5 = std::abs(
+        dofs[4]->getChildBodyNode()->getRelativeTransform().translation()[2]);
+
+    // This offset will be taken care of with footTfInv
+    L6 = 0.0;
+
+    hipRotation = Eigen::Isometry3d::Identity();
+    hipRotation.rotate(Eigen::AngleAxisd(
+        90 * constantsd::pi() / 180.0, Eigen::Vector3d::UnitZ()));
+
+    waist = dofs[2]->getChildBodyNode()->getTransform(
+                dofs[0]->getParentBodyNode())
+            * hipRotation;
+
+    footTfInv = Eigen::Isometry3d::Identity();
+    footTfInv.rotate(Eigen::AngleAxisd(
+        -90 * constantsd::pi() / 180.0, Eigen::Vector3d::UnitY()));
+    footTfInv
+        = footTfInv * mIK->getNode()->getTransform(dofs[5]->getChildBodyNode());
+    footTfInv = footTfInv.inverse();
+
+    alternatives << 1, 1, 1, 1, 1, -1, 1, -1, 1, 1, -1, -1, -1, 1, 1, -1, 1, -1,
+        -1, -1, 1, -1, -1, -1;
+
+    for (std::size_t i = 0; i < 6; ++i) {
+      dofs[i]->setPosition(saved_q[i]);
+      mDofs.push_back(dofs[i]->getIndexInSkeleton());
+    }
+
+    configured = true;
+  }
+
+  mutable double L4, L5, L6;
+  mutable Eigen::Isometry3d waist;
+  mutable Eigen::Isometry3d hipRotation;
+  mutable Eigen::Isometry3d footTfInv;
+  mutable Eigen::Matrix<int, 8, 3> alternatives;
+
+  mutable std::vector<std::size_t> mDofs;
+
+  mutable bool configured;
+
+  std::string mBaseLinkName;
+
+  mutable WeakBodyNodePtr mBaseLink;
+};
+
+class TeleoperationWorld : public dart::gui::osg::WorldNode
+{
+public:
+  enum MoveEnum_t
+  {
+    MOVE_Q = 0,
+    MOVE_W,
+    MOVE_E,
+    MOVE_A,
+    MOVE_S,
+    MOVE_D,
+    MOVE_F,
+    MOVE_Z,
+
+    NUM_MOVE
+  };
+
+  TeleoperationWorld(WorldPtr _world, SkeletonPtr _robot)
+    : dart::gui::osg::WorldNode(_world),
+      mHubo(_robot),
+      iter(0),
+      l_foot(_robot->getEndEffector("l_foot")),
+      r_foot(_robot->getEndEffector("r_foot")),
+      l_hand(_robot->getEndEffector("l_hand")),
+      r_hand(_robot->getEndEffector("r_hand"))
+  {
+    mMoveComponents.resize(NUM_MOVE, false);
+    mAnyMovement = false;
+    mAmplifyMovement = false;
+  }
+
+  void setMovement(const std::vector<bool>& moveComponents)
+  {
+    mMoveComponents = moveComponents;
+
+    mAnyMovement = false;
+
+    for (bool move : mMoveComponents) {
+      if (move) {
+        mAnyMovement = true;
+        break;
+      }
+    }
+  }
+
+  void customPreRefresh() override
+  {
+    if (mAnyMovement) {
+      Eigen::Isometry3d old_tf = mHubo->getBodyNode(0)->getWorldTransform();
+      Eigen::Isometry3d new_tf = Eigen::Isometry3d::Identity();
+      Eigen::Vector3d forward = old_tf.linear().col(0);
+      forward[2] = 0.0;
+      if (forward.norm() > 1e-10)
+        forward.normalize();
+      else
+        forward.setZero();
+
+      Eigen::Vector3d left = old_tf.linear().col(1);
+      left[2] = 0.0;
+      if (left.norm() > 1e-10)
+        left.normalize();
+      else
+        left.setZero();
+
+      const Eigen::Vector3d& up = Eigen::Vector3d::UnitZ();
+
+      double linearStep = 0.01;
+      double elevationStep = 0.2 * linearStep;
+      double rotationalStep = 2.0 * constantsd::pi() / 180.0;
+
+      if (mAmplifyMovement) {
+        linearStep *= 2.0;
+        elevationStep *= 2.0;
+        rotationalStep *= 2.0;
+      }
+
+      if (mMoveComponents[MOVE_W])
+        new_tf.translate(linearStep * forward);
+
+      if (mMoveComponents[MOVE_S])
+        new_tf.translate(-linearStep * forward);
+
+      if (mMoveComponents[MOVE_A])
+        new_tf.translate(linearStep * left);
+
+      if (mMoveComponents[MOVE_D])
+        new_tf.translate(-linearStep * left);
+
+      if (mMoveComponents[MOVE_F])
+        new_tf.translate(elevationStep * up);
+
+      if (mMoveComponents[MOVE_Z])
+        new_tf.translate(-elevationStep * up);
+
+      if (mMoveComponents[MOVE_Q])
+        new_tf.rotate(Eigen::AngleAxisd(rotationalStep, up));
+
+      if (mMoveComponents[MOVE_E])
+        new_tf.rotate(Eigen::AngleAxisd(-rotationalStep, up));
+
+      new_tf.pretranslate(old_tf.translation());
+      new_tf.rotate(old_tf.rotation());
+
+      mHubo->getJoint(0)->setPositions(FreeJoint::convertToPositions(new_tf));
+    }
+
+    mHubo->getIK(true)->solveAndApply(true);
+  }
+
+  bool mAmplifyMovement;
+
+protected:
+  SkeletonPtr mHubo;
+  std::size_t iter;
+
+  EndEffectorPtr l_foot;
+  EndEffectorPtr r_foot;
+
+  EndEffectorPtr l_hand;
+  EndEffectorPtr r_hand;
+
+  std::vector<IK::Analytical::Solution> mSolutions;
+
+  Eigen::VectorXd grad;
+
+  // Order: q, w, e, a, s, d
+  std::vector<bool> mMoveComponents;
+
+  bool mAnyMovement;
+};
+
+class InputHandler : public ::osgGA::GUIEventHandler
+{
+public:
+  InputHandler(
+      dart::gui::osg::Viewer* viewer,
+      TeleoperationWorld* teleop,
+      const SkeletonPtr& hubo,
+      const WorldPtr& world)
+    : mViewer(viewer), mTeleop(teleop), mHubo(hubo), mWorld(world)
+  {
+    initialize();
+  }
+
+  void initialize()
+  {
+    mRestConfig = mHubo->getPositions();
+
+    for (std::size_t i = 0; i < mHubo->getNumEndEffectors(); ++i) {
+      const InverseKinematicsPtr ik = mHubo->getEndEffector(i)->getIK();
+      if (ik) {
+        mDefaultBounds.push_back(ik->getErrorMethod().getBounds());
+        mDefaultTargetTf.push_back(ik->getTarget()->getRelativeTransform());
+        mConstraintActive.push_back(false);
+        mEndEffectorIndex.push_back(i);
+      }
+    }
+
+    mPosture = std::dynamic_pointer_cast<RelaxedPosture>(
+        mHubo->getIK(true)->getObjective());
+
+    mBalance = std::dynamic_pointer_cast<dart::constraint::BalanceConstraint>(
+        mHubo->getIK(true)->getProblem()->getEqConstraint(1));
+
+    mOptimizationKey = 'r';
+
+    mMoveComponents.resize(TeleoperationWorld::NUM_MOVE, false);
+  }
+
+  bool handle(
+      const ::osgGA::GUIEventAdapter& ea, ::osgGA::GUIActionAdapter&) override
+  {
+    if (nullptr == mHubo) {
+      return false;
+    }
+
+    if (::osgGA::GUIEventAdapter::KEYDOWN == ea.getEventType()) {
+      if (ea.getKey() == 'p') {
+        for (std::size_t i = 0; i < mHubo->getNumDofs(); ++i)
+          std::cout << mHubo->getDof(i)->getName() << ": "
+                    << mHubo->getDof(i)->getPosition() << std::endl;
+        return true;
+      }
+
+      if (ea.getKey() == 't') {
+        // Reset all the positions except for x, y, and yaw
+        for (std::size_t i = 0; i < mHubo->getNumDofs(); ++i) {
+          if (i < 2 || 4 < i)
+            mHubo->getDof(i)->setPosition(mRestConfig[i]);
+        }
+        return true;
+      }
+
+      if ('1' <= ea.getKey() && ea.getKey() <= '9') {
+        std::size_t index = ea.getKey() - '1';
+        if (index < mConstraintActive.size()) {
+          EndEffector* ee = mHubo->getEndEffector(mEndEffectorIndex[index]);
+          const InverseKinematicsPtr& ik = ee->getIK();
+          if (ik && mConstraintActive[index]) {
+            mConstraintActive[index] = false;
+
+            ik->getErrorMethod().setBounds(mDefaultBounds[index]);
+            ik->getTarget()->setRelativeTransform(mDefaultTargetTf[index]);
+            mWorld->removeSimpleFrame(ik->getTarget());
+          } else if (ik) {
+            mConstraintActive[index] = true;
+
+            // Use the standard default bounds instead of our custom default
+            // bounds
+            ik->getErrorMethod().setBounds();
+            ik->getTarget()->setTransform(ee->getTransform());
+            mWorld->addSimpleFrame(ik->getTarget());
+          }
+        }
+        return true;
+      }
+
+      if ('x' == ea.getKey()) {
+        EndEffector* ee = mHubo->getEndEffector("l_foot");
+        ee->getSupport()->setActive(!ee->getSupport()->isActive());
+        return true;
+      }
+
+      if ('c' == ea.getKey()) {
+        EndEffector* ee = mHubo->getEndEffector("r_foot");
+        ee->getSupport()->setActive(!ee->getSupport()->isActive());
+        return true;
+      }
+
+      if (ea.getKey() == ::osgGA::GUIEventAdapter::KEY_Shift_L)
+        mTeleop->mAmplifyMovement = true;
+
+      switch (ea.getKey()) {
+        case 'w':
+        case 'W':
+          mMoveComponents[TeleoperationWorld::MOVE_W] = true;
+          break;
+        case 'a':
+        case 'A':
+          mMoveComponents[TeleoperationWorld::MOVE_A] = true;
+          break;
+        case 's':
+        case 'S':
+          mMoveComponents[TeleoperationWorld::MOVE_S] = true;
+          break;
+        case 'd':
+        case 'D':
+          mMoveComponents[TeleoperationWorld::MOVE_D] = true;
+          break;
+        case 'q':
+        case 'Q':
+          mMoveComponents[TeleoperationWorld::MOVE_Q] = true;
+          break;
+        case 'e':
+        case 'E':
+          mMoveComponents[TeleoperationWorld::MOVE_E] = true;
+          break;
+        case 'f':
+        case 'F':
+          mMoveComponents[TeleoperationWorld::MOVE_F] = true;
+          break;
+        case 'z':
+        case 'Z':
+          mMoveComponents[TeleoperationWorld::MOVE_Z] = true;
+          break;
+      }
+
+      switch (ea.getKey()) {
+        case 'w':
+        case 'a':
+        case 's':
+        case 'd':
+        case 'q':
+        case 'e':
+        case 'f':
+        case 'z':
+        case 'W':
+        case 'A':
+        case 'S':
+        case 'D':
+        case 'Q':
+        case 'E':
+        case 'F':
+        case 'Z': {
+          mTeleop->setMovement(mMoveComponents);
+          return true;
+        }
+      }
+
+      if (mOptimizationKey == ea.getKey()) {
+        if (mPosture)
+          mPosture->enforceIdealPosture = true;
+
+        if (mBalance)
+          mBalance->setErrorMethod(
+              dart::constraint::BalanceConstraint::OPTIMIZE_BALANCE);
+
+        return true;
+      }
+    }
+
+    if (::osgGA::GUIEventAdapter::KEYUP == ea.getEventType()) {
+      if (ea.getKey() == mOptimizationKey) {
+        if (mPosture)
+          mPosture->enforceIdealPosture = false;
+
+        if (mBalance)
+          mBalance->setErrorMethod(
+              dart::constraint::BalanceConstraint::FROM_CENTROID);
+
+        return true;
+      }
+
+      if (ea.getKey() == ::osgGA::GUIEventAdapter::KEY_Shift_L)
+        mTeleop->mAmplifyMovement = false;
+
+      switch (ea.getKey()) {
+        case 'w':
+        case 'W':
+          mMoveComponents[TeleoperationWorld::MOVE_W] = false;
+          break;
+        case 'a':
+        case 'A':
+          mMoveComponents[TeleoperationWorld::MOVE_A] = false;
+          break;
+        case 's':
+        case 'S':
+          mMoveComponents[TeleoperationWorld::MOVE_S] = false;
+          break;
+        case 'd':
+        case 'D':
+          mMoveComponents[TeleoperationWorld::MOVE_D] = false;
+          break;
+        case 'q':
+        case 'Q':
+          mMoveComponents[TeleoperationWorld::MOVE_Q] = false;
+          break;
+        case 'e':
+        case 'E':
+          mMoveComponents[TeleoperationWorld::MOVE_E] = false;
+          break;
+        case 'f':
+        case 'F':
+          mMoveComponents[TeleoperationWorld::MOVE_F] = false;
+          break;
+        case 'z':
+        case 'Z':
+          mMoveComponents[TeleoperationWorld::MOVE_Z] = false;
+          break;
+      }
+
+      switch (ea.getKey()) {
+        case 'w':
+        case 'a':
+        case 's':
+        case 'd':
+        case 'q':
+        case 'e':
+        case 'f':
+        case 'z':
+        case 'W':
+        case 'A':
+        case 'S':
+        case 'D':
+        case 'Q':
+        case 'E':
+        case 'F':
+        case 'Z': {
+          mTeleop->setMovement(mMoveComponents);
+          return true;
+        }
+      }
+    }
+
+    return false;
+  }
+
+protected:
+  dart::gui::osg::Viewer* mViewer;
+
+  TeleoperationWorld* mTeleop;
+
+  SkeletonPtr mHubo;
+
+  WorldPtr mWorld;
+
+  Eigen::VectorXd mRestConfig;
+
+  std::vector<bool> mConstraintActive;
+
+  std::vector<std::size_t> mEndEffectorIndex;
+
+  std::vector<std::pair<Eigen::Vector6d, Eigen::Vector6d>> mDefaultBounds;
+
+  dart::common::aligned_vector<Eigen::Isometry3d> mDefaultTargetTf;
+
+  std::shared_ptr<RelaxedPosture> mPosture;
+
+  std::shared_ptr<dart::constraint::BalanceConstraint> mBalance;
+
+  char mOptimizationKey;
+
+  std::vector<bool> mMoveComponents;
+};
+
+SkeletonPtr createGround()
+{
+  // Create a Skeleton to represent the ground
+  SkeletonPtr ground = Skeleton::create("ground");
+  Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
+  double thickness = 0.01;
+  tf.translation() = Eigen::Vector3d(0, 0, -thickness / 2.0);
+  WeldJoint::Properties joint;
+  joint.mT_ParentBodyToJoint = tf;
+  ground->createJointAndBodyNodePair<WeldJoint>(nullptr, joint);
+  ShapePtr groundShape
+      = std::make_shared<BoxShape>(Eigen::Vector3d(10, 10, thickness));
+
+  auto shapeNode = ground->getBodyNode(0)
+                       ->createShapeNodeWith<
+                           VisualAspect,
+                           CollisionAspect,
+                           DynamicsAspect>(groundShape);
+  shapeNode->getVisualAspect()->setColor(dart::Color::Blue(0.2));
+
+  return ground;
+}
+
+SkeletonPtr createHubo()
+{
+  dart::utils::DartLoader loader;
+  loader.addPackageDirectory("drchubo", DART_DATA_PATH "/urdf/drchubo");
+  SkeletonPtr hubo
+      = loader.parseSkeleton(DART_DATA_PATH "/urdf/drchubo/drchubo.urdf");
+
+  for (std::size_t i = 0; i < hubo->getNumBodyNodes(); ++i) {
+    BodyNode* bn = hubo->getBodyNode(i);
+    if (bn->getName().substr(0, 7) == "Body_LF"
+        || bn->getName().substr(0, 7) == "Body_RF") {
+      bn->remove();
+      --i;
+    }
+  }
+
+  return hubo;
+}
+
+void setStartupConfiguration(const SkeletonPtr& hubo)
+{
+  hubo->getDof("LHP")->setPosition(toRadian(-45.0));
+  hubo->getDof("LKP")->setPosition(toRadian(90.0));
+  hubo->getDof("LAP")->setPosition(toRadian(-45.0));
+
+  hubo->getDof("RHP")->setPosition(toRadian(-45.0));
+  hubo->getDof("RKP")->setPosition(toRadian(90.0));
+  hubo->getDof("RAP")->setPosition(toRadian(-45.0));
+
+  hubo->getDof("LSP")->setPosition(toRadian(30.0));
+  hubo->getDof("LEP")->setPosition(toRadian(-120.0));
+
+  hubo->getDof("RSP")->setPosition(toRadian(30.0));
+  hubo->getDof("REP")->setPosition(toRadian(-120.0));
+
+  hubo->getDof("LSY")->setPositionLowerLimit(toRadian(-90.0));
+  hubo->getDof("LSY")->setPositionUpperLimit(toRadian(90.0));
+  hubo->getDof("LWY")->setPositionLowerLimit(toRadian(-90.0));
+  hubo->getDof("LWY")->setPositionUpperLimit(toRadian(90.0));
+
+  hubo->getDof("RSY")->setPositionLowerLimit(toRadian(-90.0));
+  hubo->getDof("RSY")->setPositionUpperLimit(toRadian(90.0));
+  hubo->getDof("RWY")->setPositionLowerLimit(toRadian(-90.0));
+  hubo->getDof("RWY")->setPositionUpperLimit(toRadian(90.0));
+}
+
+void setupEndEffectors(const SkeletonPtr& hubo)
+{
+  Eigen::VectorXd rootjoint_weights = Eigen::VectorXd::Ones(7);
+  rootjoint_weights = 0.01 * rootjoint_weights;
+
+  double extra_error_clamp = 0.1;
+
+  Eigen::Vector3d linearBounds
+      = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
+
+  Eigen::Vector3d angularBounds
+      = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
+
+  Eigen::Isometry3d tf_hand(Eigen::Isometry3d::Identity());
+  tf_hand.translate(Eigen::Vector3d(0.0, 0.0, -0.09));
+
+  EndEffector* l_hand
+      = hubo->getBodyNode("Body_LWR")->createEndEffector("l_hand");
+  l_hand->setDefaultRelativeTransform(tf_hand, true);
+
+  dart::gui::osg::InteractiveFramePtr lh_target(
+      new dart::gui::osg::InteractiveFrame(Frame::World(), "lh_target"));
+
+  l_hand->getIK(true)->setTarget(lh_target);
+  l_hand->getIK()->useWholeBody();
+
+  l_hand->getIK()->setGradientMethod<HuboArmIK>("Body_LSP");
+
+  l_hand->getIK()->getAnalytical()->setExtraDofUtilization(
+      IK::Analytical::POST_ANALYTICAL);
+
+  l_hand->getIK()->getAnalytical()->setExtraErrorLengthClamp(extra_error_clamp);
+
+  l_hand->getIK()->getGradientMethod().setComponentWeights(rootjoint_weights);
+
+  l_hand->getIK()->getErrorMethod().setLinearBounds(
+      -linearBounds, linearBounds);
+
+  l_hand->getIK()->getErrorMethod().setAngularBounds(
+      -angularBounds, angularBounds);
+
+  EndEffector* r_hand
+      = hubo->getBodyNode("Body_RWR")->createEndEffector("r_hand");
+  r_hand->setDefaultRelativeTransform(tf_hand, true);
+
+  dart::gui::osg::InteractiveFramePtr rh_target(
+      new dart::gui::osg::InteractiveFrame(Frame::World(), "rh_target"));
+
+  r_hand->getIK(true)->setTarget(rh_target);
+  r_hand->getIK()->useWholeBody();
+
+  r_hand->getIK()->setGradientMethod<HuboArmIK>("Body_RSP");
+
+  r_hand->getIK()->getAnalytical()->setExtraDofUtilization(
+      IK::Analytical::POST_ANALYTICAL);
+
+  r_hand->getIK()->getAnalytical()->setExtraErrorLengthClamp(extra_error_clamp);
+
+  r_hand->getIK()->getGradientMethod().setComponentWeights(rootjoint_weights);
+
+  r_hand->getIK()->getErrorMethod().setLinearBounds(
+      -linearBounds, linearBounds);
+
+  r_hand->getIK()->getErrorMethod().setAngularBounds(
+      -angularBounds, angularBounds);
+
+  dart::math::SupportGeometry foot_support;
+  foot_support.push_back(Eigen::Vector3d(-0.08, 0.05, 0.0));
+  foot_support.push_back(Eigen::Vector3d(-0.18, 0.05, 0.0));
+  foot_support.push_back(Eigen::Vector3d(-0.18, -0.05, 0.0));
+  foot_support.push_back(Eigen::Vector3d(-0.08, -0.05, 0.0));
+
+  Eigen::Isometry3d tf_foot(Eigen::Isometry3d::Identity());
+  double ground_dist = 0.01;
+  tf_foot.translation() = Eigen::Vector3d(0.14, 0.0, -0.136 + ground_dist);
+
+  linearBounds[2] = 1e-8;
+  Eigen::Vector3d ground_offset = ground_dist * Eigen::Vector3d::UnitZ();
+
+  angularBounds[0] = 1e-8;
+  angularBounds[1] = 1e-8;
+
+  EndEffector* l_foot
+      = hubo->getBodyNode("Body_LAR")->createEndEffector("l_foot");
+  l_foot->setDefaultRelativeTransform(tf_foot, true);
+
+  dart::gui::osg::InteractiveFramePtr lf_target(
+      new dart::gui::osg::InteractiveFrame(Frame::World(), "lf_target"));
+
+  l_foot->getIK(true)->setTarget(lf_target);
+
+  l_foot->getIK()->setHierarchyLevel(1);
+
+  l_foot->getIK()->getErrorMethod().setLinearBounds(
+      -linearBounds + ground_offset, linearBounds + ground_offset);
+  l_foot->getIK()->getErrorMethod().setAngularBounds(
+      -angularBounds, angularBounds);
+
+  l_foot->getIK()->setGradientMethod<HuboLegIK>("Body_LHY");
+
+  l_foot->getSupport(true)->setGeometry(foot_support);
+  l_foot->getSupport()->setActive();
+
+  EndEffector* r_foot
+      = hubo->getBodyNode("Body_RAR")->createEndEffector("r_foot");
+  r_foot->setDefaultRelativeTransform(tf_foot, true);
+
+  dart::gui::osg::InteractiveFramePtr rf_target(
+      new dart::gui::osg::InteractiveFrame(Frame::World(), "rf_target"));
+
+  r_foot->getIK(true)->setTarget(rf_target);
+
+  r_foot->getIK()->setHierarchyLevel(1);
+
+  r_foot->getIK()->getErrorMethod().setLinearBounds(
+      -linearBounds + ground_offset, linearBounds + ground_offset);
+  r_foot->getIK()->getErrorMethod().setAngularBounds(
+      -angularBounds, angularBounds);
+
+  r_foot->getIK()->setGradientMethod<HuboLegIK>("Body_RHY");
+
+  r_foot->getSupport(true)->setGeometry(foot_support);
+  r_foot->getSupport()->setActive();
+
+  dart::math::SupportGeometry peg_support;
+  peg_support.push_back(Eigen::Vector3d::Zero());
+
+  linearBounds
+      = Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
+  angularBounds = linearBounds;
+
+  Eigen::Isometry3d tf_peg(Eigen::Isometry3d::Identity());
+  tf_peg.translation() = Eigen::Vector3d(0.0, 0.0, 0.09);
+
+  EndEffector* l_peg
+      = hubo->getBodyNode("Body_LWP")->createEndEffector("l_peg");
+  l_peg->setDefaultRelativeTransform(tf_peg, true);
+
+  dart::gui::osg::InteractiveFramePtr lp_target(
+      new dart::gui::osg::InteractiveFrame(Frame::World(), "lp_target"));
+
+  l_peg->getIK(true)->setTarget(lp_target);
+
+  l_peg->getIK()->setGradientMethod<HuboArmIK>("Body_LSP");
+
+  l_peg->getIK()->getErrorMethod().setLinearBounds(-linearBounds, linearBounds);
+  l_peg->getIK()->getErrorMethod().setAngularBounds(
+      -angularBounds, angularBounds);
+
+  l_peg->getSupport(true)->setGeometry(peg_support);
+
+  EndEffector* r_peg
+      = hubo->getBodyNode("Body_RWP")->createEndEffector("r_peg");
+  r_peg->setDefaultRelativeTransform(tf_peg, true);
+
+  dart::gui::osg::InteractiveFramePtr rp_target(
+      new dart::gui::osg::InteractiveFrame(Frame::World(), "rp_target"));
+
+  r_peg->getIK(true)->setTarget(rp_target);
+
+  r_peg->getIK()->setGradientMethod<HuboArmIK>("Body_RSP");
+
+  r_peg->getIK()->getErrorMethod().setLinearBounds(-linearBounds, linearBounds);
+  r_peg->getIK()->getErrorMethod().setAngularBounds(
+      -angularBounds, angularBounds);
+
+  r_peg->getSupport(true)->setGeometry(peg_support);
+
+  double heightChange
+      = -r_foot->getWorldTransform().translation()[2] + ground_dist;
+  hubo->getDof("rootJoint_pos_z")->setPosition(heightChange);
+
+  l_foot->getIK()->getTarget()->setTransform(l_foot->getTransform());
+  r_foot->getIK()->getTarget()->setTransform(r_foot->getTransform());
+}
+
+void enableDragAndDrops(dart::gui::osg::Viewer& viewer, const SkeletonPtr& hubo)
+{
+  // Turn on drag-and-drop for the whole Skeleton
+  for (std::size_t i = 0; i < hubo->getNumBodyNodes(); ++i)
+    viewer.enableDragAndDrop(hubo->getBodyNode(i), false, false);
+
+  for (std::size_t i = 0; i < hubo->getNumEndEffectors(); ++i) {
+    EndEffector* ee = hubo->getEndEffector(i);
+    if (!ee->getIK())
+      continue;
+
+    // Check whether the target is an interactive frame, and add it if it is
+    if (const auto& frame
+        = std::dynamic_pointer_cast<dart::gui::osg::InteractiveFrame>(
+            ee->getIK()->getTarget()))
+      viewer.enableDragAndDrop(frame.get());
+  }
+}
+
+void setupWholeBodySolver(const SkeletonPtr& hubo)
+{
+  std::shared_ptr<dart::optimizer::GradientDescentSolver> solver
+      = std::dynamic_pointer_cast<dart::optimizer::GradientDescentSolver>(
+          hubo->getIK(true)->getSolver());
+
+  std::size_t nDofs = hubo->getNumDofs();
+
+  double default_weight = 0.01;
+  Eigen::VectorXd weights = default_weight * Eigen::VectorXd::Ones(nDofs);
+  weights[2] = 0.0;
+  weights[3] = 0.0;
+  weights[4] = 0.0;
+
+  Eigen::VectorXd lower_posture = Eigen::VectorXd::Constant(
+      nDofs, -std::numeric_limits<double>::infinity());
+  lower_posture[0] = -0.35;
+  lower_posture[1] = -0.35;
+  lower_posture[5] = 0.55;
+
+  Eigen::VectorXd upper_posture = Eigen::VectorXd::Constant(
+      nDofs, std::numeric_limits<double>::infinity());
+  upper_posture[0] = 0.35;
+  upper_posture[1] = 0.50;
+  upper_posture[5] = 0.95;
+
+  std::shared_ptr<RelaxedPosture> objective = std::make_shared<RelaxedPosture>(
+      hubo->getPositions(), lower_posture, upper_posture, weights);
+
+  hubo->getIK()->setObjective(objective);
+
+  std::shared_ptr<dart::constraint::BalanceConstraint> balance
+      = std::make_shared<dart::constraint::BalanceConstraint>(hubo->getIK());
+  hubo->getIK()->getProblem()->addEqConstraint(balance);
+
+  balance->setErrorMethod(dart::constraint::BalanceConstraint::FROM_CENTROID);
+  balance->setBalanceMethod(dart::constraint::BalanceConstraint::SHIFT_SUPPORT);
+
+  solver->setNumMaxIterations(5);
+}
+
+int main()
+{
+  dart::simulation::WorldPtr world(new dart::simulation::World);
+
+  SkeletonPtr hubo = createHubo();
+  setStartupConfiguration(hubo);
+  setupEndEffectors(hubo);
+
+  Eigen::VectorXd positions = hubo->getPositions();
+  // We make a clone to test whether the cloned version behaves the exact same
+  // as the original version.
+  hubo = hubo->cloneSkeleton("hubo_copy");
+  hubo->setPositions(positions);
+
+  world->addSkeleton(hubo);
+  world->addSkeleton(createGround());
+
+  setupWholeBodySolver(hubo);
+
+  ::osg::ref_ptr<TeleoperationWorld> node = new TeleoperationWorld(world, hubo);
+
+  dart::gui::osg::Viewer viewer;
+  viewer.allowSimulation(false);
+  viewer.addWorldNode(node);
+
+  enableDragAndDrops(viewer, hubo);
+
+  viewer.addEventHandler(new InputHandler(&viewer, node, hubo, world));
+
+  double display_elevation = 0.05;
+  viewer.addAttachment(
+      new dart::gui::osg::SupportPolygonVisual(hubo, display_elevation));
+
+  std::cout << viewer.getInstructions() << std::endl;
+
+  std::cout
+      << "Alt + Click:   Try to translate a body without changing its "
+         "orientation\n"
+      << "Ctrl + Click:  Try to rotate a body without changing its "
+         "translation\n"
+      << "Shift + Click: Move a body using only its parent joint\n"
+      << "1 -> 6:        Toggle the interactive target of an EndEffector\n"
+      << "W A S D:       Move the robot around the scene\n"
+      << "Q E:           Rotate the robot counter-clockwise and clockwise\n"
+      << "F Z:           Shift the robot's elevation up and down\n"
+      << "X C:           Toggle support on the left and right foot\n"
+      << "R:             Optimize the robot's posture\n"
+      << "T:             Reset the robot to its relaxed posture\n\n"
+      << "  The green polygon is the support polygon of the robot, and the "
+         "blue/red ball is\n"
+      << "  the robot's center of mass. The green ball is the centroid of the "
+         "polygon.\n\n"
+      << "Note that this is purely kinematic. Physical simulation is not "
+         "allowed in this app.\n"
+      << std::endl;
+
+  // Set up the window
+  viewer.setUpViewInWindow(0, 0, 1280, 960);
+
+  // Set up the default viewing position
+  viewer.getCameraManipulator()->setHomePosition(
+      ::osg::Vec3(5.34, 3.00, 1.91),
+      ::osg::Vec3(0.00, 0.00, 0.50),
+      ::osg::Vec3(-0.20, -0.08, 0.98));
+
+  // Reset the camera manipulator so that it starts in the new viewing position
+  viewer.setCameraManipulator(viewer.getCameraManipulator());
+
+  viewer.run();
+}