dartpy 7.0.0.dev0__cp313-cp313-manylinux_2_39_x86_64.whl

include/dart/dynamics/detail/GenericJointAspect.hpp

@@ -0,0 +1,353 @@
+/*
+ * 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.
+ */
+
+#ifndef DART_DYNAMICS_DETAIL_GenericJointASPECT_HPP_
+#define DART_DYNAMICS_DETAIL_GenericJointASPECT_HPP_
+
+#include <dart/dynamics/Joint.hpp>
+
+#include <dart/math/Helpers.hpp>
+
+#include <dart/common/AspectWithVersion.hpp>
+
+namespace dart {
+namespace dynamics {
+
+// Forward declare the GenericJoint class
+template <class ConfigSpaceT>
+class GenericJoint;
+
+namespace detail {
+
+//==============================================================================
+template <class ConfigSpaceT>
+struct GenericJointState
+{
+  constexpr static std::size_t NumDofs = ConfigSpaceT::NumDofs;
+  using EuclideanPoint = typename ConfigSpaceT::EuclideanPoint;
+  using Vector = typename ConfigSpaceT::Vector;
+
+  /// Position
+  EuclideanPoint mPositions;
+
+  /// Generalized velocity
+  Vector mVelocities;
+
+  /// Generalized acceleration
+  Vector mAccelerations;
+
+  /// Generalized force
+  Vector mForces;
+
+  /// Command
+  Vector mCommands;
+
+  GenericJointState(
+      const EuclideanPoint& positions = EuclideanPoint::Zero(),
+      const Vector& velocities = Vector::Zero(),
+      const Vector& accelerations = Vector::Zero(),
+      const Vector& forces = Vector::Zero(),
+      const Vector& commands = Vector::Zero());
+
+  virtual ~GenericJointState() = default;
+
+  // To get byte-aligned Eigen vectors
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//==============================================================================
+template <class ConfigSpaceT>
+struct GenericJointUniqueProperties
+{
+  constexpr static std::size_t NumDofs = ConfigSpaceT::NumDofs;
+  using EuclideanPoint = typename ConfigSpaceT::EuclideanPoint;
+  using Vector = typename ConfigSpaceT::Vector;
+  using BoolArray = std::array<bool, NumDofs>;
+  using StringArray = std::array<std::string, NumDofs>;
+
+  /// Lower limit of position
+  EuclideanPoint mPositionLowerLimits;
+
+  /// Upper limit of position
+  EuclideanPoint mPositionUpperLimits;
+
+  /// Initial positions
+  EuclideanPoint mInitialPositions;
+
+  /// Min value allowed.
+  Vector mVelocityLowerLimits;
+
+  /// Max value allowed.
+  Vector mVelocityUpperLimits;
+
+  /// Initial velocities
+  Vector mInitialVelocities;
+
+  /// Min value allowed.
+  Vector mAccelerationLowerLimits;
+
+  /// upper limit of generalized acceleration
+  Vector mAccelerationUpperLimits;
+
+  /// Min value allowed.
+  Vector mForceLowerLimits;
+
+  /// Max value allowed.
+  Vector mForceUpperLimits;
+
+  /// Joint spring stiffness
+  Vector mSpringStiffnesses;
+
+  /// Rest joint position for joint spring
+  EuclideanPoint mRestPositions;
+
+  /// Joint damping coefficient
+  Vector mDampingCoefficients;
+
+  /// Joint Coulomb friction
+  Vector mFrictions;
+
+  /// True if the name of the corresponding DOF is not allowed to be
+  /// overwritten
+  BoolArray mPreserveDofNames;
+
+  /// The name of the DegreesOfFreedom for this Joint
+  StringArray mDofNames;
+
+  /// Default constructor
+  GenericJointUniqueProperties(
+      const EuclideanPoint& positionLowerLimits
+      = EuclideanPoint::Constant(-math::constantsd::inf()),
+      const EuclideanPoint& positionUpperLimits
+      = EuclideanPoint::Constant(math::constantsd::inf()),
+      const EuclideanPoint& initialPositions = EuclideanPoint::Zero(),
+      const Vector& velocityLowerLimits
+      = Vector::Constant(-math::constantsd::inf()),
+      const Vector& velocityUpperLimits
+      = Vector::Constant(math::constantsd::inf()),
+      const Vector& initialVelocities = Vector::Zero(),
+      const Vector& accelerationLowerLimits
+      = Vector::Constant(-math::constantsd::inf()),
+      const Vector& accelerationUpperLimits
+      = Vector::Constant(math::constantsd::inf()),
+      const Vector& forceLowerLimits
+      = Vector::Constant(-math::constantsd::inf()),
+      const Vector& forceUpperLimits
+      = Vector::Constant(math::constantsd::inf()),
+      const Vector& springStiffness = Vector::Zero(),
+      const EuclideanPoint& restPosition = EuclideanPoint::Zero(),
+      const Vector& dampingCoefficient = Vector::Zero(),
+      const Vector& coulombFrictions = Vector::Zero());
+
+  /// Copy constructor
+  // Note: we only need this because VS2013 lacks full support for std::array
+  // Once std::array is properly supported, this should be removed.
+  GenericJointUniqueProperties(const GenericJointUniqueProperties& other);
+
+  virtual ~GenericJointUniqueProperties() = default;
+
+  /// Copy assignment operator
+  // Note: we only need this because VS2013 lacks full support for std::array
+  // Once std::array is properly supported, this should be removed.
+  GenericJointUniqueProperties& operator=(
+      const GenericJointUniqueProperties& other);
+
+public:
+  // To get byte-aligned Eigen vectors
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//==============================================================================
+template <class ConfigSpaceT>
+struct GenericJointProperties : Joint::Properties,
+                                GenericJointUniqueProperties<ConfigSpaceT>
+{
+  GenericJointProperties(
+      const Joint::Properties& jointProperties = Joint::Properties(),
+      const GenericJointUniqueProperties<ConfigSpaceT>& genericProperties
+      = GenericJointUniqueProperties<ConfigSpaceT>());
+
+  virtual ~GenericJointProperties() = default;
+
+public:
+  // To get byte-aligned Eigen vectors
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//==============================================================================
+//
+// These namespace-level definitions are required to enable ODR-use of static
+// constexpr member variables.
+//
+// See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426
+//
+template <class ConfigSpaceT>
+constexpr std::size_t GenericJointState<ConfigSpaceT>::NumDofs;
+
+template <class ConfigSpaceT>
+constexpr std::size_t GenericJointUniqueProperties<ConfigSpaceT>::NumDofs;
+
+//==============================================================================
+template <class ConfigSpaceT>
+GenericJointState<ConfigSpaceT>::GenericJointState(
+    const EuclideanPoint& positions,
+    const Vector& velocities,
+    const Vector& accelerations,
+    const Vector& forces,
+    const Vector& commands)
+  : mPositions(positions),
+    mVelocities(velocities),
+    mAccelerations(accelerations),
+    mForces(forces),
+    mCommands(commands)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <class ConfigSpaceT>
+GenericJointUniqueProperties<ConfigSpaceT>::GenericJointUniqueProperties(
+    const EuclideanPoint& positionLowerLimits,
+    const EuclideanPoint& positionUpperLimits,
+    const EuclideanPoint& initialPositions,
+    const Vector& velocityLowerLimits,
+    const Vector& velocityUpperLimits,
+    const Vector& initialVelocities,
+    const Vector& accelerationLowerLimits,
+    const Vector& accelerationUpperLimits,
+    const Vector& forceLowerLimits,
+    const Vector& forceUpperLimits,
+    const Vector& springStiffness,
+    const EuclideanPoint& restPosition,
+    const Vector& dampingCoefficient,
+    const Vector& coulombFrictions)
+  : mPositionLowerLimits(positionLowerLimits),
+    mPositionUpperLimits(positionUpperLimits),
+    mInitialPositions(initialPositions),
+    mVelocityLowerLimits(velocityLowerLimits),
+    mVelocityUpperLimits(velocityUpperLimits),
+    mInitialVelocities(initialVelocities),
+    mAccelerationLowerLimits(accelerationLowerLimits),
+    mAccelerationUpperLimits(accelerationUpperLimits),
+    mForceLowerLimits(forceLowerLimits),
+    mForceUpperLimits(forceUpperLimits),
+    mSpringStiffnesses(springStiffness),
+    mRestPositions(restPosition),
+    mDampingCoefficients(dampingCoefficient),
+    mFrictions(coulombFrictions)
+{
+  for (auto i = 0u; i < NumDofs; ++i) {
+    mPreserveDofNames[i] = false;
+    mDofNames[i] = std::string();
+  }
+}
+
+//==============================================================================
+template <class ConfigSpaceT>
+GenericJointUniqueProperties<ConfigSpaceT>::GenericJointUniqueProperties(
+    const GenericJointUniqueProperties& _other)
+  : mPositionLowerLimits(_other.mPositionLowerLimits),
+    mPositionUpperLimits(_other.mPositionUpperLimits),
+    mInitialPositions(_other.mInitialPositions),
+    mVelocityLowerLimits(_other.mVelocityLowerLimits),
+    mVelocityUpperLimits(_other.mVelocityUpperLimits),
+    mInitialVelocities(_other.mInitialVelocities),
+    mAccelerationLowerLimits(_other.mAccelerationLowerLimits),
+    mAccelerationUpperLimits(_other.mAccelerationUpperLimits),
+    mForceLowerLimits(_other.mForceLowerLimits),
+    mForceUpperLimits(_other.mForceUpperLimits),
+    mSpringStiffnesses(_other.mSpringStiffnesses),
+    mRestPositions(_other.mRestPositions),
+    mDampingCoefficients(_other.mDampingCoefficients),
+    mFrictions(_other.mFrictions)
+{
+  for (auto i = 0u; i < NumDofs; ++i) {
+    mPreserveDofNames[i] = _other.mPreserveDofNames[i];
+    mDofNames[i] = _other.mDofNames[i];
+  }
+}
+
+//==============================================================================
+template <class ConfigSpaceT>
+GenericJointUniqueProperties<ConfigSpaceT>&
+GenericJointUniqueProperties<ConfigSpaceT>::operator=(
+    const GenericJointUniqueProperties& other)
+{
+  if (this != &other) {
+    mPositionLowerLimits = other.mPositionLowerLimits;
+    mPositionUpperLimits = other.mPositionUpperLimits;
+    mInitialPositions = other.mInitialPositions;
+    mVelocityLowerLimits = other.mVelocityLowerLimits;
+    mVelocityUpperLimits = other.mVelocityUpperLimits;
+    mInitialVelocities = other.mInitialVelocities;
+    mAccelerationLowerLimits = other.mAccelerationLowerLimits;
+    mAccelerationUpperLimits = other.mAccelerationUpperLimits;
+    mForceLowerLimits = other.mForceLowerLimits;
+    mForceUpperLimits = other.mForceUpperLimits;
+    mSpringStiffnesses = other.mSpringStiffnesses;
+    mRestPositions = other.mRestPositions;
+    mDampingCoefficients = other.mDampingCoefficients;
+    mFrictions = other.mFrictions;
+
+    for (auto i = 0u; i < NumDofs; ++i) {
+      mPreserveDofNames[i] = other.mPreserveDofNames[i];
+      mDofNames[i] = other.mDofNames[i];
+    }
+  }
+  return *this;
+}
+
+//==============================================================================
+template <class ConfigSpaceT>
+GenericJointProperties<ConfigSpaceT>::GenericJointProperties(
+    const Joint::Properties& jointProperties,
+    const GenericJointUniqueProperties<ConfigSpaceT>& genericProperties)
+  : Joint::Properties(jointProperties),
+    GenericJointUniqueProperties<ConfigSpaceT>(genericProperties)
+{
+  // Do nothing
+}
+
+//==============================================================================
+template <class Derived, class ConfigSpaceT>
+using GenericJointBase = common::EmbedStateAndPropertiesOnTopOf<
+    Derived,
+    GenericJointState<ConfigSpaceT>,
+    GenericJointUniqueProperties<ConfigSpaceT>,
+    Joint>;
+
+} // namespace detail
+
+} // namespace dynamics
+} // namespace dart
+
+#endif // DART_DYNAMICS_DETAIL_GenericJointASPECT_HPP_

include/dart/dynamics/detail/HeightmapShape-impl.hpp

@@ -0,0 +1,243 @@
+/*
+ * 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/dynamics/BoxShape.hpp>
+#include <dart/dynamics/HeightmapShape.hpp>
+
+#include <dart/common/Console.hpp>
+
+#include <algorithm>
+#include <limits>
+
+#include <cmath>
+
+namespace dart {
+namespace dynamics {
+
+//==============================================================================
+template <typename S>
+HeightmapShape<S>::HeightmapShape() : Shape(HEIGHTMAP), mScale(1, 1, 1)
+{
+  static_assert(
+      std::is_same<S, float>::value || std::is_same<S, double>::value,
+      "Height field needs to be double or float");
+}
+
+//==============================================================================
+template <typename S>
+const std::string& HeightmapShape<S>::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+template <typename S>
+const std::string& HeightmapShape<S>::getStaticType()
+{
+  static const std::string type
+      = "HeightmapShape (" + std::string(typeid(S).name()) + ")";
+  return type;
+}
+
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::setScale(const Vector3& scale)
+{
+  assert(scale[0] > 0.0);
+  assert(scale[1] > 0.0);
+  assert(scale[2] > 0.0);
+  mScale = scale;
+  mIsBoundingBoxDirty = true;
+  mIsVolumeDirty = true;
+
+  incrementVersion();
+}
+
+//==============================================================================
+template <typename S>
+auto HeightmapShape<S>::getScale() const -> const Vector3&
+{
+  return mScale;
+}
+
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::setHeightField(
+    const std::size_t& width,
+    const std::size_t& depth,
+    const std::vector<S>& heights)
+{
+  assert(heights.size() == width * depth);
+  if ((width * depth) != heights.size()) {
+    dterr << "[HeightmapShape] Size of height field needs to be width*depth="
+          << width * depth << "\n";
+    return;
+  }
+  if (heights.empty()) {
+    dtwarn << "Empty height field makes no sense.\n";
+    return;
+  }
+
+  // make heightmap data local copy
+  const Eigen::Map<const HeightField> data(heights.data(), depth, width);
+
+  setHeightField(data);
+}
+
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::setHeightField(const HeightField& heights)
+{
+  mHeights = heights;
+
+  mMinHeight = heights.minCoeff();
+  mMaxHeight = heights.maxCoeff();
+
+  mIsBoundingBoxDirty = true;
+  mIsVolumeDirty = true;
+
+  incrementVersion();
+}
+
+//==============================================================================
+template <typename S>
+auto HeightmapShape<S>::getHeightField() const -> const HeightField&
+{
+  return mHeights;
+}
+
+//==============================================================================
+template <typename S>
+auto HeightmapShape<S>::getHeightFieldModifiable() const -> HeightField&
+{
+  return mHeights;
+}
+
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::flipY() const
+{
+  mHeights = mHeights.colwise().reverse().eval();
+}
+
+//==============================================================================
+template <typename S>
+auto HeightmapShape<S>::getMaxHeight() const -> S
+{
+  return mMaxHeight;
+}
+
+//==============================================================================
+template <typename S>
+auto HeightmapShape<S>::getMinHeight() const -> S
+{
+  return mMinHeight;
+}
+
+//==============================================================================
+template <typename S>
+std::size_t HeightmapShape<S>::getWidth() const
+{
+  return mHeights.cols();
+}
+
+//==============================================================================
+template <typename S>
+std::size_t HeightmapShape<S>::getDepth() const
+{
+  return mHeights.rows();
+}
+
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::notifyColorUpdated(const Eigen::Vector4d& /*color*/)
+{
+  incrementVersion();
+}
+
+//==============================================================================
+template <typename S>
+ShapePtr HeightmapShape<S>::clone() const
+{
+  auto new_mesh = std::make_shared<HeightmapShape<S>>();
+  new_mesh->mScale = mScale;
+  new_mesh->setHeightField(mHeights);
+
+  return new_mesh;
+}
+
+//==============================================================================
+template <typename S>
+Eigen::Matrix3d HeightmapShape<S>::computeInertia(double mass) const
+{
+  if (mIsBoundingBoxDirty) {
+    updateBoundingBox();
+  }
+  return BoxShape::computeInertia(getBoundingBox().computeFullExtents(), mass);
+}
+
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::computeBoundingBox(
+    Eigen::Vector3d& min, Eigen::Vector3d& max) const
+{
+  const double dimX = getWidth() * mScale.x();
+  const double dimY = getDepth() * mScale.y();
+  const double dimZ = (mMaxHeight - mMinHeight) * mScale.z();
+  min = Eigen::Vector3d(-dimX * 0.5, -dimY * 0.5, mMinHeight * mScale.z());
+  max = min + Eigen::Vector3d(dimX, dimY, dimZ);
+}
+
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::updateBoundingBox() const
+{
+  Eigen::Vector3d min;
+  Eigen::Vector3d max;
+  computeBoundingBox(min, max);
+  mBoundingBox.setMin(min);
+  mBoundingBox.setMax(max);
+  mIsBoundingBoxDirty = false;
+}
+
+//==============================================================================
+template <typename S>
+void HeightmapShape<S>::updateVolume() const
+{
+  updateBoundingBox();
+  const Eigen::Vector3d size = mBoundingBox.getMax() - mBoundingBox.getMin();
+  mVolume = size.x() * size.y() * size.z();
+  mIsVolumeDirty = false;
+}
+
+} // namespace dynamics
+} // namespace dart

include/dart/dynamics/detail/InverseKinematics.hpp

@@ -0,0 +1,83 @@
+/*
+ * 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.
+ */
+
+#ifndef DART_DYNAMICS_DETAIL_INVERSEKINEMATICS_HPP_
+#define DART_DYNAMICS_DETAIL_INVERSEKINEMATICS_HPP_
+
+#include <dart/dynamics/InverseKinematics.hpp>
+
+#include <type_traits>
+
+namespace dart {
+namespace dynamics {
+
+//==============================================================================
+template <class IKErrorMethod, typename... Args>
+IKErrorMethod& InverseKinematics::setErrorMethod(Args&&... args)
+{
+  IKErrorMethod* newMethod
+      = new IKErrorMethod(this, std::forward<Args>(args)...);
+  mErrorMethod = std::unique_ptr<ErrorMethod>(newMethod);
+  return *newMethod;
+}
+
+//==============================================================================
+template <class IKGradientMethod, typename... Args>
+IKGradientMethod& InverseKinematics::setGradientMethod(Args&&... args)
+{
+  IKGradientMethod* newMethod
+      = new IKGradientMethod(this, std::forward<Args>(args)...);
+  mGradientMethod = std::unique_ptr<GradientMethod>(newMethod);
+
+  mAnalytical = dynamic_cast<Analytical*>(mGradientMethod.get());
+  if (nullptr != mAnalytical)
+    mAnalytical->constructDofMap();
+
+  return *newMethod;
+}
+
+//==============================================================================
+template <class DegreeOfFreedomT>
+void InverseKinematics::setDofs(const std::vector<DegreeOfFreedomT*>& _dofs)
+{
+  std::vector<std::size_t> indices;
+  indices.reserve(_dofs.size());
+  for (const DegreeOfFreedomT* dof : _dofs)
+    indices.push_back(dof->getIndexInSkeleton());
+
+  setDofs(indices);
+}
+
+} // namespace dynamics
+} // namespace dart
+
+#endif // DART_DYNAMICS_DETAIL_INVERSEKINEMATICS_HPP_