GenericJoint.hpp

Go to the documentation of this file.

/*
 * 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.
 */
 
#ifndef DART_DYNAMICS_DETAIL_GenericJoint_HPP_
#define DART_DYNAMICS_DETAIL_GenericJoint_HPP_
 
#include "dart/config.hpp"
#include "dart/dynamics/BodyNode.hpp"
#include "dart/dynamics/DegreeOfFreedom.hpp"
#include "dart/dynamics/GenericJoint.hpp"
#include "dart/math/ConfigurationSpace.hpp"
#include "dart/math/Helpers.hpp"
 
#define GenericJoint_REPORT_DIM_MISMATCH(func, arg)                            \
  {                                                                            \
    dterr << "[GenericJoint::" #func "] Mismatch beteween size of "            \
          << #arg " [" << arg.size() << "] and the number of "                 \
          << "DOFs [" << getNumDofs() << "] for Joint named ["                 \
          << this->getName() << "].\n";                                        \
    assert(false);                                                             \
  }
 
#define GenericJoint_REPORT_OUT_OF_RANGE(func, index)                          \
  {                                                                            \
    dterr << "[GenericJoint::" << #func << "] The index [" << index            \
          << "] is out of range for Joint named [" << this->getName()          \
          << "] which has " << this->getNumDofs() << " DOFs.\n";               \
    assert(false);                                                             \
  }
 
#define GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(func)                         \
  {                                                                            \
    dterr << "[GenericJoint::" #func "] Unsupported actuator type ("           \
          << Joint::mAspectProperties.mActuatorType << ") for Joint ["         \
          << this->getName() << "].\n";                                        \
    assert(false);                                                             \
  }
 
#define GenericJoint_SET_IF_DIFFERENT(mField, value)                           \
  if (value == Base::mAspectProperties.mField)                                 \
    return;                                                                    \
  Base::mAspectProperties.mField = value;                                      \
  Joint::incrementVersion();
 
namespace dart {
namespace dynamics {
 
//==============================================================================
//
// 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 size_t GenericJoint<ConfigSpaceT>::NumDofs;
 
//==============================================================================
template <class ConfigSpaceT>
GenericJoint<ConfigSpaceT>::~GenericJoint()
{
  for (auto i = 0u; i < NumDofs; ++i)
    delete mDofs[i];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setProperties(const Properties& properties)
{
  Joint::setProperties(static_cast<const Joint::Properties&>(properties));
  setProperties(static_cast<const UniqueProperties&>(properties));
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setProperties(
    const UniqueProperties& properties)
{
  setAspectProperties(properties);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setAspectState(const AspectState& state)
{
  setCommands(state.mCommands);
  setPositionsStatic(state.mPositions);
  setVelocitiesStatic(state.mVelocities);
  setAccelerationsStatic(state.mAccelerations);
  setForces(state.mForces);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setAspectProperties(
    const AspectProperties& properties)
{
  for (auto i = 0u; i < NumDofs; ++i)
  {
    setDofName(i, properties.mDofNames[i], properties.mPreserveDofNames[i]);
    setPositionLowerLimit(i, properties.mPositionLowerLimits[i]);
    setPositionUpperLimit(i, properties.mPositionUpperLimits[i]);
    setInitialPosition(i, properties.mInitialPositions[i]);
    setVelocityLowerLimit(i, properties.mVelocityLowerLimits[i]);
    setVelocityUpperLimit(i, properties.mVelocityUpperLimits[i]);
    setInitialVelocity(i, properties.mInitialVelocities[i]);
    setAccelerationLowerLimit(i, properties.mAccelerationLowerLimits[i]);
    setAccelerationUpperLimit(i, properties.mAccelerationUpperLimits[i]);
    setForceLowerLimit(i, properties.mForceLowerLimits[i]);
    setForceUpperLimit(i, properties.mForceUpperLimits[i]);
    setSpringStiffness(i, properties.mSpringStiffnesses[i]);
    setRestPosition(i, properties.mRestPositions[i]);
    setDampingCoefficient(i, properties.mDampingCoefficients[i]);
    setCoulombFriction(i, properties.mFrictions[i]);
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
typename GenericJoint<ConfigSpaceT>::Properties
GenericJoint<ConfigSpaceT>::getGenericJointProperties() const
{
  return GenericJoint<ConfigSpaceT>::Properties(
      Joint::mAspectProperties, Base::mAspectProperties);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::copy(const GenericJoint<ConfigSpaceT>& other)
{
  if (this == &other)
    return;
 
  setProperties(other.getGenericJointProperties());
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::copy(const GenericJoint<ConfigSpaceT>* other)
{
  if (nullptr == other)
    return;
 
  copy(*other);
}
 
//==============================================================================
template <class ConfigSpaceT>
GenericJoint<ConfigSpaceT>& GenericJoint<ConfigSpaceT>::operator=(
    const GenericJoint<ConfigSpaceT>& other)
{
  copy(other);
  return *this;
}
 
//==============================================================================
template <class ConfigSpaceT>
DegreeOfFreedom* GenericJoint<ConfigSpaceT>::getDof(size_t index)
{
  if (index < NumDofs)
    return mDofs[index];
 
  GenericJoint_REPORT_OUT_OF_RANGE(getDof, index);
 
  return nullptr;
}
 
//==============================================================================
template <class ConfigSpaceT>
const DegreeOfFreedom* GenericJoint<ConfigSpaceT>::getDof(size_t index) const
{
  if (index < NumDofs)
    return mDofs[index];
 
  GenericJoint_REPORT_OUT_OF_RANGE(getDof, index);
 
  return nullptr;
}
 
//==============================================================================
template <class ConfigSpaceT>
size_t GenericJoint<ConfigSpaceT>::getNumDofs() const
{
  return NumDofs;
}
 
//==============================================================================
template <class ConfigSpaceT>
const std::string& GenericJoint<ConfigSpaceT>::setDofName(
    size_t index, const std::string& name, bool preserveName)
{
  if (NumDofs <= index)
  {
    dterr << "[GenericJoint::setDofName] Attempting to set the name of DOF "
          << "index " << index << ", which is out of bounds for the Joint ["
          << this->getName()
          << "]. We will set the name of DOF index 0 instead.\n";
    assert(false);
    index = 0u;
  }
 
  preserveDofName(index, preserveName);
 
  std::string& dofName = Base::mAspectProperties.mDofNames[index];
 
  if (name == dofName)
    return dofName;
 
  const SkeletonPtr& skel
      = this->mChildBodyNode ? this->mChildBodyNode->getSkeleton() : nullptr;
  if (skel)
    dofName = skel->mNameMgrForDofs.changeObjectName(mDofs[index], name);
  else
    dofName = name;
 
  return dofName;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::preserveDofName(size_t index, bool preserve)
{
  if (NumDofs <= index)
  {
    GenericJoint_REPORT_OUT_OF_RANGE(preserveDofName, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mPreserveDofNames[index], preserve);
}
 
//==============================================================================
template <class ConfigSpaceT>
bool GenericJoint<ConfigSpaceT>::isDofNamePreserved(size_t index) const
{
  if (NumDofs <= index)
  {
    GenericJoint_REPORT_OUT_OF_RANGE(isDofNamePreserved, index);
    index = 0;
  }
 
  return Base::mAspectProperties.mPreserveDofNames[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
const std::string& GenericJoint<ConfigSpaceT>::getDofName(size_t index) const
{
  if (NumDofs <= index)
  {
    dterr << "[GenericJoint::getDofName] Requested name of DOF index [" << index
          << "] in Joint [" << this->getName() << "], but that is "
          << "out of bounds (max " << NumDofs - 1
          << "). Returning name of DOF 0.\n";
    assert(false);
    return Base::mAspectProperties.mDofNames[0];
  }
 
  return Base::mAspectProperties.mDofNames[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
size_t GenericJoint<ConfigSpaceT>::getIndexInSkeleton(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getIndexInSkeleton, index);
    return 0;
  }
 
  return mDofs[index]->mIndexInSkeleton;
}
 
//==============================================================================
template <class ConfigSpaceT>
size_t GenericJoint<ConfigSpaceT>::getIndexInTree(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getIndexInTree, index);
    return 0;
  }
 
  return mDofs[index]->mIndexInTree;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setCommand(size_t index, double command)
{
  if (index >= getNumDofs())
    GenericJoint_REPORT_OUT_OF_RANGE(setCommand, index);
 
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
      this->mAspectState.mCommands[index] = math::clip(
          command,
          Base::mAspectProperties.mForceLowerLimits[index],
          Base::mAspectProperties.mForceUpperLimits[index]);
      break;
    case Joint::PASSIVE:
      if (0.0 != command)
      {
        dtwarn << "[GenericJoint::setCommand] Attempting to set a non-zero ("
               << command << ") command for a PASSIVE joint ["
               << this->getName() << "].\n";
      }
      this->mAspectState.mCommands[index] = command;
      break;
    case Joint::SERVO:
      this->mAspectState.mCommands[index] = math::clip(
          command,
          Base::mAspectProperties.mVelocityLowerLimits[index],
          Base::mAspectProperties.mVelocityUpperLimits[index]);
      break;
    case Joint::MIMIC:
      if (0.0 != command)
      {
        dtwarn << "[GenericJoint::setCommand] Attempting to set a non-zero ("
               << command << ") command for a MIMIC joint [" << this->getName()
               << "].\n";
      }
      this->mAspectState.mCommands[index] = math::clip(
          command,
          Base::mAspectProperties.mVelocityLowerLimits[index],
          Base::mAspectProperties.mVelocityUpperLimits[index]);
      break;
    case Joint::ACCELERATION:
      this->mAspectState.mCommands[index] = math::clip(
          command,
          Base::mAspectProperties.mAccelerationLowerLimits[index],
          Base::mAspectProperties.mAccelerationUpperLimits[index]);
      break;
    case Joint::VELOCITY:
      this->mAspectState.mCommands[index] = math::clip(
          command,
          Base::mAspectProperties.mVelocityLowerLimits[index],
          Base::mAspectProperties.mVelocityUpperLimits[index]);
      // TODO: This possibly makes the acceleration to exceed the limits.
      break;
    case Joint::LOCKED:
      if (0.0 != command)
      {
        dtwarn << "[GenericJoint::setCommand] Attempting to set a non-zero ("
               << command << ") command for a LOCKED joint [" << this->getName()
               << "].\n";
      }
      this->mAspectState.mCommands[index] = command;
      break;
    default:
      assert(false);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getCommand(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getCommand, index);
    return 0.0;
  }
 
  return this->mAspectState.mCommands[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setCommands(const Eigen::VectorXd& commands)
{
  if (static_cast<size_t>(commands.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setCommands, commands);
    return;
  }
 
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
      this->mAspectState.mCommands = math::clip(
          commands,
          Base::mAspectProperties.mForceLowerLimits,
          Base::mAspectProperties.mForceUpperLimits);
      break;
    case Joint::PASSIVE:
      if (Vector::Zero() != commands)
      {
        dtwarn << "[GenericJoint::setCommands] Attempting to set a non-zero ("
               << commands.transpose() << ") command for a PASSIVE joint ["
               << this->getName() << "].\n";
      }
      this->mAspectState.mCommands = commands;
      break;
    case Joint::SERVO:
      this->mAspectState.mCommands = math::clip(
          commands,
          Base::mAspectProperties.mVelocityLowerLimits,
          Base::mAspectProperties.mVelocityUpperLimits);
      break;
    case Joint::MIMIC:
      if (Vector::Zero() != commands)
      {
        dtwarn << "[GenericJoint::setCommands] Attempting to set a non-zero ("
               << commands.transpose() << ") command for a MIMIC joint ["
               << this->getName() << "].\n";
      }
      this->mAspectState.mCommands = math::clip(
          commands,
          Base::mAspectProperties.mVelocityLowerLimits,
          Base::mAspectProperties.mVelocityUpperLimits);
      break;
    case Joint::ACCELERATION:
      this->mAspectState.mCommands = math::clip(
          commands,
          Base::mAspectProperties.mAccelerationLowerLimits,
          Base::mAspectProperties.mAccelerationUpperLimits);
      break;
    case Joint::VELOCITY:
      this->mAspectState.mCommands = math::clip(
          commands,
          Base::mAspectProperties.mVelocityLowerLimits,
          Base::mAspectProperties.mVelocityUpperLimits);
      // TODO: This possibly makes the acceleration to exceed the limits.
      break;
    case Joint::LOCKED:
      if (Vector::Zero() != commands)
      {
        dtwarn << "[GenericJoint::setCommands] Attempting to set a non-zero ("
               << commands.transpose() << ") command for a LOCKED joint ["
               << this->getName() << "].\n";
      }
      this->mAspectState.mCommands = commands;
      break;
    default:
      assert(false);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getCommands() const
{
  return this->mAspectState.mCommands;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetCommands()
{
  this->mAspectState.mCommands.setZero();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setPosition(size_t index, double position)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setPosition, index);
    return;
  }
 
  if (this->mAspectState.mPositions[index] == position)
    return;
  // TODO(JS): Above code should be changed something like:
  //  if (ConfigSpaceT::getEuclideanPoint(mPositions, index) == position)
  //    return;
 
  // Note: It would not make much sense to use setPositionsStatic() here
  this->mAspectState.mPositions[index] = position;
  this->notifyPositionUpdated();
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getPosition(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getPosition, index);
    return 0.0;
  }
 
  return getPositionsStatic()[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setPositions(const Eigen::VectorXd& positions)
{
  if (static_cast<size_t>(positions.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setPositions, positions);
    return;
  }
 
  setPositionsStatic(positions);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getPositions() const
{
  return getPositionsStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setPositionLowerLimit(
    size_t index, double position)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setPositionLowerLimit, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mPositionLowerLimits[index], position);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getPositionLowerLimit(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getPositionLowerLimit, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mPositionLowerLimits[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setPositionLowerLimits(
    const Eigen::VectorXd& lowerLimits)
{
  if (static_cast<size_t>(lowerLimits.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setPositionLowerLimits, lowerLimits);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mPositionLowerLimits, lowerLimits);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getPositionLowerLimits() const
{
  return Base::mAspectProperties.mPositionLowerLimits;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setPositionUpperLimit(
    size_t index, double position)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setPositionUpperLimit, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mPositionUpperLimits[index], position);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getPositionUpperLimit(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getPositionUpperLimit, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mPositionUpperLimits[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setPositionUpperLimits(
    const Eigen::VectorXd& upperLimits)
{
  if (static_cast<size_t>(upperLimits.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setPositionUpperLimits, upperLimits);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mPositionUpperLimits, upperLimits);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getPositionUpperLimits() const
{
  return Base::mAspectProperties.mPositionUpperLimits;
}
 
//==============================================================================
template <class ConfigSpaceT>
bool GenericJoint<ConfigSpaceT>::hasPositionLimit(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(hasPositionLimit, index);
    return true;
  }
 
  return std::isfinite(Base::mAspectProperties.mPositionUpperLimits[index])
         || std::isfinite(Base::mAspectProperties.mPositionLowerLimits[index]);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetPosition(size_t index)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(resetPosition, index);
    return;
  }
 
  setPosition(index, Base::mAspectProperties.mInitialPositions[index]);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetPositions()
{
  setPositionsStatic(Base::mAspectProperties.mInitialPositions);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setInitialPosition(
    size_t index, double initial)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setInitialPosition, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mInitialPositions[index], initial);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getInitialPosition(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getInitialPosition, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mInitialPositions[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setInitialPositions(
    const Eigen::VectorXd& initial)
{
  if (static_cast<size_t>(initial.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setInitialPositions, initial);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mInitialPositions, initial);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getInitialPositions() const
{
  return Base::mAspectProperties.mInitialPositions;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setPositionsStatic(const Vector& positions)
{
  if (this->mAspectState.mPositions == positions)
    return;
 
  this->mAspectState.mPositions = positions;
  this->notifyPositionUpdated();
}
 
//==============================================================================
template <class ConfigSpaceT>
const typename GenericJoint<ConfigSpaceT>::Vector&
GenericJoint<ConfigSpaceT>::getPositionsStatic() const
{
  return this->mAspectState.mPositions;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setVelocitiesStatic(const Vector& velocities)
{
  if (this->mAspectState.mVelocities == velocities)
    return;
 
  this->mAspectState.mVelocities = velocities;
  this->notifyVelocityUpdated();
}
 
//==============================================================================
template <class ConfigSpaceT>
const typename GenericJoint<ConfigSpaceT>::Vector&
GenericJoint<ConfigSpaceT>::getVelocitiesStatic() const
{
  return this->mAspectState.mVelocities;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setAccelerationsStatic(const Vector& accels)
{
  if (this->mAspectState.mAccelerations == accels)
    return;
 
  this->mAspectState.mAccelerations = accels;
  this->notifyAccelerationUpdated();
}
 
//==============================================================================
template <class ConfigSpaceT>
const typename GenericJoint<ConfigSpaceT>::Vector&
GenericJoint<ConfigSpaceT>::getAccelerationsStatic() const
{
  return this->mAspectState.mAccelerations;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setVelocity(size_t index, double velocity)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setVelocity, index);
    return;
  }
 
  if (this->mAspectState.mVelocities[index] == velocity)
    return;
 
  // Note: It would not make much sense to use setVelocitiesStatic() here
  this->mAspectState.mVelocities[index] = velocity;
  this->notifyVelocityUpdated();
 
  if (Joint::mAspectProperties.mActuatorType == Joint::VELOCITY)
    this->mAspectState.mCommands[index] = this->getVelocitiesStatic()[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getVelocity(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getVelocity, index);
    return 0.0;
  }
 
  return getVelocitiesStatic()[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setVelocities(
    const Eigen::VectorXd& velocities)
{
  if (static_cast<size_t>(velocities.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setVelocities, velocities);
    return;
  }
 
  setVelocitiesStatic(velocities);
 
  if (Joint::mAspectProperties.mActuatorType == Joint::VELOCITY)
    this->mAspectState.mCommands = this->getVelocitiesStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getVelocities() const
{
  return getVelocitiesStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setVelocityLowerLimit(
    size_t index, double velocity)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setVelocityLowerLimit, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mVelocityLowerLimits[index], velocity);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getVelocityLowerLimit(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getVelocityLowerLimit, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mVelocityLowerLimits[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setVelocityLowerLimits(
    const Eigen::VectorXd& lowerLimits)
{
  if (static_cast<size_t>(lowerLimits.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setVelocityLowerLimits, lowerLimits);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mVelocityLowerLimits, lowerLimits);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getVelocityLowerLimits() const
{
  return Base::mAspectProperties.mVelocityLowerLimits;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setVelocityUpperLimit(
    size_t index, double velocity)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setVelocityUpperLimit, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mVelocityUpperLimits[index], velocity);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getVelocityUpperLimit(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getVelocityUpperLimit, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mVelocityUpperLimits[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setVelocityUpperLimits(
    const Eigen::VectorXd& upperLimits)
{
  if (static_cast<size_t>(upperLimits.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setVelocityUpperLimits, upperLimits);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mVelocityUpperLimits, upperLimits);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getVelocityUpperLimits() const
{
  return Base::mAspectProperties.mVelocityUpperLimits;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetVelocity(size_t index)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(resetVelocity, index);
    return;
  }
 
  setVelocity(index, Base::mAspectProperties.mInitialVelocities[index]);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetVelocities()
{
  setVelocitiesStatic(Base::mAspectProperties.mInitialVelocities);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setInitialVelocity(
    size_t index, double initial)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setInitialVelocity, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mInitialVelocities[index], initial);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getInitialVelocity(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getInitialVelocity, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mInitialVelocities[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setInitialVelocities(
    const Eigen::VectorXd& initial)
{
  if (static_cast<size_t>(initial.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setInitialVelocities, initial);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mInitialVelocities, initial);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getInitialVelocities() const
{
  return Base::mAspectProperties.mInitialVelocities;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setAcceleration(
    size_t index, double acceleration)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setAcceleration, index);
    return;
  }
 
  if (this->mAspectState.mAccelerations[index] == acceleration)
    return;
 
  // Note: It would not make much sense to use setAccelerationsStatic() here
  this->mAspectState.mAccelerations[index] = acceleration;
  this->notifyAccelerationUpdated();
 
  if (Joint::mAspectProperties.mActuatorType == Joint::ACCELERATION)
    this->mAspectState.mCommands[index] = this->getAccelerationsStatic()[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getAcceleration(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getAcceleration, index);
    return 0.0;
  }
 
  return getAccelerationsStatic()[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setAccelerations(
    const Eigen::VectorXd& accelerations)
{
  if (static_cast<size_t>(accelerations.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setAccelerations, accelerations);
    return;
  }
 
  setAccelerationsStatic(accelerations);
 
  if (Joint::mAspectProperties.mActuatorType == Joint::ACCELERATION)
    this->mAspectState.mCommands = this->getAccelerationsStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getAccelerations() const
{
  return getAccelerationsStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setAccelerationLowerLimit(
    size_t index, double acceleration)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setAccelerationLowerLimit, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mAccelerationLowerLimits[index], acceleration);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getAccelerationLowerLimit(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getAccelerationLowerLimit, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mAccelerationLowerLimits[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setAccelerationLowerLimits(
    const Eigen::VectorXd& lowerLimits)
{
  if (static_cast<size_t>(lowerLimits.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setAccelerationLowerLimits, lowerLimits);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mAccelerationLowerLimits, lowerLimits);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getAccelerationLowerLimits() const
{
  return Base::mAspectProperties.mAccelerationLowerLimits;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setAccelerationUpperLimit(
    size_t index, double acceleration)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setAccelerationUpperLimit, index) return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mAccelerationUpperLimits[index], acceleration);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getAccelerationUpperLimit(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getAccelerationUpperLimit, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mAccelerationUpperLimits[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setAccelerationUpperLimits(
    const Eigen::VectorXd& upperLimits)
{
  if (static_cast<size_t>(upperLimits.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setAccelerationUpperLimits, upperLimits);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mAccelerationUpperLimits, upperLimits);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getAccelerationUpperLimits() const
{
  return Base::mAspectProperties.mAccelerationUpperLimits;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetAccelerations()
{
  setAccelerationsStatic(Vector::Zero());
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setForce(size_t index, double force)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setForce, index);
    return;
  }
 
  this->mAspectState.mForces[index] = force;
 
  if (Joint::mAspectProperties.mActuatorType == Joint::FORCE)
    this->mAspectState.mCommands[index] = this->mAspectState.mForces[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getForce(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getForce, index);
    return 0.0;
  }
 
  return this->mAspectState.mForces[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setForces(const Eigen::VectorXd& forces)
{
  if (static_cast<size_t>(forces.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setForces, forces);
    return;
  }
 
  this->mAspectState.mForces = forces;
 
  if (Joint::mAspectProperties.mActuatorType == Joint::FORCE)
    this->mAspectState.mCommands = this->mAspectState.mForces;
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getForces() const
{
  return this->mAspectState.mForces;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setForceLowerLimit(size_t index, double force)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setForceLowerLimit, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mForceLowerLimits[index], force);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getForceLowerLimit(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getForceLowerLimit, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mForceLowerLimits[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setForceLowerLimits(
    const Eigen::VectorXd& lowerLimits)
{
  if (static_cast<size_t>(lowerLimits.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setForceLowerLimits, lowerLimits);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mForceLowerLimits, lowerLimits);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getForceLowerLimits() const
{
  return Base::mAspectProperties.mForceLowerLimits;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setForceUpperLimit(size_t index, double force)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setForceUpperLimit, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mForceUpperLimits[index], force);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getForceUpperLimit(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getForceUpperLimit, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mForceUpperLimits[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setForceUpperLimits(
    const Eigen::VectorXd& upperLimits)
{
  if (static_cast<size_t>(upperLimits.size()) != getNumDofs())
  {
    GenericJoint_REPORT_DIM_MISMATCH(setForceUpperLimits, upperLimits);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mForceUpperLimits, upperLimits);
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getForceUpperLimits() const
{
  return Base::mAspectProperties.mForceUpperLimits;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetForces()
{
  this->mAspectState.mForces.setZero();
 
  if (Joint::mAspectProperties.mActuatorType == Joint::FORCE)
    this->mAspectState.mCommands = this->mAspectState.mForces;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setVelocityChange(
    size_t index, double velocityChange)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setVelocityChange, index);
    return;
  }
 
  mVelocityChanges[index] = velocityChange;
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getVelocityChange(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getVelocityChange, index);
    return 0.0;
  }
 
  return mVelocityChanges[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetVelocityChanges()
{
  mVelocityChanges.setZero();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setConstraintImpulse(
    size_t index, double impulse)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setConstraintImpulse, index);
    return;
  }
 
  mConstraintImpulses[index] = impulse;
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getConstraintImpulse(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getConstraintImpulse, index);
    return 0.0;
  }
 
  return mConstraintImpulses[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetConstraintImpulses()
{
  mConstraintImpulses.setZero();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::integratePositions(double dt)
{
  const Point& point = math::integratePosition<ConfigSpaceT>(
      math::toManifoldPoint<ConfigSpaceT>(getPositionsStatic()),
      getVelocitiesStatic(),
      dt);
 
  setPositionsStatic(math::toEuclideanPoint<ConfigSpaceT>(point));
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::integrateVelocities(double dt)
{
  setVelocitiesStatic(math::integrateVelocity<ConfigSpaceT>(
      getVelocitiesStatic(), getAccelerationsStatic(), dt));
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getPositionDifferences(
    const Eigen::VectorXd& q2, const Eigen::VectorXd& q1) const
{
  if (static_cast<size_t>(q1.size()) != getNumDofs()
      || static_cast<size_t>(q2.size()) != getNumDofs())
  {
    dterr << "[GenericJoint::getPositionsDifference] q1's size [" << q1.size()
          << "] or q2's size [" << q2.size() << "] must both equal the dof ["
          << this->getNumDofs() << "] for Joint [" << this->getName() << "].\n";
    assert(false);
    return Eigen::VectorXd::Zero(getNumDofs());
  }
 
  return getPositionDifferencesStatic(q2, q1);
}
 
//==============================================================================
template <class ConfigSpaceT>
typename ConfigSpaceT::Vector
GenericJoint<ConfigSpaceT>::getPositionDifferencesStatic(
    const Vector& q2, const Vector& q1) const
{
  return q2 - q1;
  // TODO(JS): Move this implementation to each configuration space classes.
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setSpringStiffness(size_t index, double k)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setSpringStiffness, index);
    return;
  }
 
  assert(k >= 0.0);
 
  GenericJoint_SET_IF_DIFFERENT(mSpringStiffnesses[index], k);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getSpringStiffness(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getSpringStiffness, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mSpringStiffnesses[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setRestPosition(size_t index, double q0)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setRestPosition, index);
    return;
  }
 
  GenericJoint_SET_IF_DIFFERENT(mRestPositions[index], q0);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getRestPosition(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getRestPosition, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mRestPositions[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setDampingCoefficient(size_t index, double d)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setDampingCoefficient, index);
    return;
  }
 
  assert(d >= 0.0);
 
  GenericJoint_SET_IF_DIFFERENT(mDampingCoefficients[index], d);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getDampingCoefficient(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getDampingCoefficient, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mDampingCoefficients[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setCoulombFriction(
    size_t index, double friction)
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(setCoulombFriction, index);
    return;
  }
 
  assert(friction >= 0.0);
 
  GenericJoint_SET_IF_DIFFERENT(mFrictions[index], friction);
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::getCoulombFriction(size_t index) const
{
  if (index >= getNumDofs())
  {
    GenericJoint_REPORT_OUT_OF_RANGE(getCoulombFriction, index);
    return 0.0;
  }
 
  return Base::mAspectProperties.mFrictions[index];
}
 
//==============================================================================
template <class ConfigSpaceT>
double GenericJoint<ConfigSpaceT>::computePotentialEnergy() const
{
  // Spring energy
  Vector displacement
      = getPositionsStatic() - Base::mAspectProperties.mRestPositions;
 
  const double pe = 0.5
                    * displacement.dot(
                        Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
                            displacement));
 
  return pe;
}
 
//==============================================================================
template <class ConfigSpaceT>
const math::Jacobian GenericJoint<ConfigSpaceT>::getRelativeJacobian() const
{
  return getRelativeJacobianStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
const typename GenericJoint<ConfigSpaceT>::JacobianMatrix&
GenericJoint<ConfigSpaceT>::getRelativeJacobianStatic() const
{
  if (this->mIsRelativeJacobianDirty)
  {
    this->updateRelativeJacobian(false);
    this->mIsRelativeJacobianDirty = false;
  }
 
  return mJacobian;
}
 
//==============================================================================
template <class ConfigSpaceT>
math::Jacobian GenericJoint<ConfigSpaceT>::getRelativeJacobian(
    const Eigen::VectorXd& positions) const
{
  return getRelativeJacobianStatic(positions);
}
 
//==============================================================================
template <class ConfigSpaceT>
const math::Jacobian GenericJoint<ConfigSpaceT>::getRelativeJacobianTimeDeriv()
    const
{
  return getRelativeJacobianTimeDerivStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
const typename GenericJoint<ConfigSpaceT>::JacobianMatrix&
GenericJoint<ConfigSpaceT>::getRelativeJacobianTimeDerivStatic() const
{
  if (this->mIsRelativeJacobianTimeDerivDirty)
  {
    this->updateRelativeJacobianTimeDeriv();
    this->mIsRelativeJacobianTimeDerivDirty = false;
  }
 
  return mJacobianDeriv;
}
 
//==============================================================================
template <class ConfigSpaceT>
GenericJoint<ConfigSpaceT>::GenericJoint(const Properties& properties)
  : mVelocityChanges(Vector::Zero()),
    mImpulses(Vector::Zero()),
    mConstraintImpulses(Vector::Zero()),
    mJacobian(JacobianMatrix::Zero()),
    mJacobianDeriv(JacobianMatrix::Zero()),
    mInvProjArtInertia(Matrix::Zero()),
    mInvProjArtInertiaImplicit(Matrix::Zero()),
    mTotalForce(Vector::Zero()),
    mTotalImpulse(Vector::Zero())
{
  for (auto i = 0u; i < NumDofs; ++i)
    mDofs[i] = this->createDofPointer(i);
 
  // Joint and GenericJoint Aspects must be created by the most derived class.
  this->mAspectState.mPositions = properties.mInitialPositions;
  this->mAspectState.mVelocities = properties.mInitialVelocities;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::registerDofs()
{
  const SkeletonPtr& skel = this->mChildBodyNode->getSkeleton();
  for (auto i = 0u; i < NumDofs; ++i)
  {
    Base::mAspectProperties.mDofNames[i]
        = skel->mNameMgrForDofs.issueNewNameAndAdd(
            mDofs[i]->getName(), mDofs[i]);
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::Vector6d GenericJoint<ConfigSpaceT>::getBodyConstraintWrench() const
{
  assert(this->mChildBodyNode);
  return this->mChildBodyNode->getBodyForce()
         - this->getRelativeJacobianStatic() * this->mAspectState.mForces;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateRelativeSpatialVelocity() const
{
  this->mSpatialVelocity
      = this->getRelativeJacobianStatic() * this->getVelocitiesStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateRelativeSpatialAcceleration() const
{
  this->mSpatialAcceleration = this->getRelativePrimaryAcceleration()
                               + this->getRelativeJacobianTimeDerivStatic()
                                     * this->getVelocitiesStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateRelativePrimaryAcceleration() const
{
  this->mPrimaryAcceleration
      = this->getRelativeJacobianStatic() * this->getAccelerationsStatic();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addVelocityTo(Eigen::Vector6d& vel)
{
  // Add joint velocity to _vel
  vel.noalias() += getRelativeJacobianStatic() * getVelocitiesStatic();
 
  // Verification
  assert(!math::isNan(vel));
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::setPartialAccelerationTo(
    Eigen::Vector6d& partialAcceleration, const Eigen::Vector6d& childVelocity)
{
  // ad(V, S * dq) + dS * dq
  partialAcceleration
      = math::ad(
            childVelocity, getRelativeJacobianStatic() * getVelocitiesStatic())
        + getRelativeJacobianTimeDerivStatic() * getVelocitiesStatic();
  // Verification
  assert(!math::isNan(partialAcceleration));
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addAccelerationTo(Eigen::Vector6d& acc)
{
  // Add joint acceleration to _acc
  acc.noalias() += getRelativeJacobianStatic() * getAccelerationsStatic();
 
  // Verification
  assert(!math::isNan(acc));
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addVelocityChangeTo(
    Eigen::Vector6d& velocityChange)
{
  // Add joint velocity change to velocityChange
  velocityChange.noalias() += getRelativeJacobianStatic() * mVelocityChanges;
 
  // Verification
  assert(!math::isNan(velocityChange));
}
 
//==============================================================================
template <class ConfigSpaceT>
const typename GenericJoint<ConfigSpaceT>::Matrix&
GenericJoint<ConfigSpaceT>::getInvProjArtInertia() const
{
  Joint::updateArticulatedInertia();
 
  return mInvProjArtInertia;
}
 
//==============================================================================
template <class ConfigSpaceT>
const typename GenericJoint<ConfigSpaceT>::Matrix&
GenericJoint<ConfigSpaceT>::getInvProjArtInertiaImplicit() const
{
  Joint::updateArticulatedInertia();
 
  return mInvProjArtInertiaImplicit;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildArtInertiaTo(
    Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      addChildArtInertiaToDynamic(parentArtInertia, childArtInertia);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      addChildArtInertiaToKinematic(parentArtInertia, childArtInertia);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(addChildArtInertiaTo);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildArtInertiaToDynamic(
    Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
{
  // Child body's articulated inertia
  JacobianMatrix AIS = childArtInertia * getRelativeJacobianStatic();
  Eigen::Matrix6d PI = childArtInertia;
  PI.noalias() -= AIS * mInvProjArtInertia * AIS.transpose();
  assert(!math::isNan(PI));
 
  // Add child body's articulated inertia to parent body's articulated inertia.
  // Note that mT should be updated.
  parentArtInertia
      += math::transformInertia(this->getRelativeTransform().inverse(), PI);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildArtInertiaToKinematic(
    Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
{
  // Add child body's articulated inertia to parent body's articulated inertia.
  // Note that mT should be updated.
  parentArtInertia += math::transformInertia(
      this->getRelativeTransform().inverse(), childArtInertia);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildArtInertiaImplicitTo(
    Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      addChildArtInertiaImplicitToDynamic(parentArtInertia, childArtInertia);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      addChildArtInertiaImplicitToKinematic(parentArtInertia, childArtInertia);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(addChildArtInertiaImplicitTo);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildArtInertiaImplicitToDynamic(
    Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
{
  // Child body's articulated inertia
  JacobianMatrix AIS = childArtInertia * getRelativeJacobianStatic();
  Eigen::Matrix6d PI = childArtInertia;
  PI.noalias() -= AIS * mInvProjArtInertiaImplicit * AIS.transpose();
  assert(!math::isNan(PI));
 
  // Add child body's articulated inertia to parent body's articulated inertia.
  // Note that mT should be updated.
  parentArtInertia
      += math::transformInertia(this->getRelativeTransform().inverse(), PI);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildArtInertiaImplicitToKinematic(
    Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
{
  // Add child body's articulated inertia to parent body's articulated inertia.
  // Note that mT should be updated.
  parentArtInertia += math::transformInertia(
      this->getRelativeTransform().inverse(), childArtInertia);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateInvProjArtInertia(
    const Eigen::Matrix6d& artInertia)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      updateInvProjArtInertiaDynamic(artInertia);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      updateInvProjArtInertiaKinematic(artInertia);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateInvProjArtInertia);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateInvProjArtInertiaDynamic(
    const Eigen::Matrix6d& artInertia)
{
  // Projected articulated inertia
  const JacobianMatrix& Jacobian = getRelativeJacobianStatic();
  const Matrix projAI = Jacobian.transpose() * artInertia * Jacobian;
 
  // Inversion of projected articulated inertia
  mInvProjArtInertia = math::inverse<ConfigSpaceT>(projAI);
 
  // Verification
  assert(!math::isNan(mInvProjArtInertia));
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateInvProjArtInertiaKinematic(
    const Eigen::Matrix6d& /*_artInertia*/)
{
  // Do nothing
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateInvProjArtInertiaImplicit(
    const Eigen::Matrix6d& artInertia, double timeStep)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      updateInvProjArtInertiaImplicitDynamic(artInertia, timeStep);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      updateInvProjArtInertiaImplicitKinematic(artInertia, timeStep);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateInvProjArtInertiaImplicit);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateInvProjArtInertiaImplicitDynamic(
    const Eigen::Matrix6d& artInertia, double timeStep)
{
  // Projected articulated inertia
  const JacobianMatrix& Jacobian = getRelativeJacobianStatic();
  Matrix projAI = Jacobian.transpose() * artInertia * Jacobian;
 
  // Add additional inertia for implicit damping and spring force
  projAI += (timeStep * Base::mAspectProperties.mDampingCoefficients
             + timeStep * timeStep * Base::mAspectProperties.mSpringStiffnesses)
                .asDiagonal();
 
  // Inversion of projected articulated inertia
  mInvProjArtInertiaImplicit = math::inverse<ConfigSpaceT>(projAI);
 
  // Verification
  assert(!math::isNan(mInvProjArtInertiaImplicit));
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateInvProjArtInertiaImplicitKinematic(
    const Eigen::Matrix6d& /*artInertia*/, double /*timeStep*/)
{
  // Do nothing
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildBiasForceTo(
    Eigen::Vector6d& parentBiasForce,
    const Eigen::Matrix6d& childArtInertia,
    const Eigen::Vector6d& childBiasForce,
    const Eigen::Vector6d& childPartialAcc)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      addChildBiasForceToDynamic(
          parentBiasForce, childArtInertia, childBiasForce, childPartialAcc);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      addChildBiasForceToKinematic(
          parentBiasForce, childArtInertia, childBiasForce, childPartialAcc);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(addChildBiasForceTo);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildBiasForceToDynamic(
    Eigen::Vector6d& parentBiasForce,
    const Eigen::Matrix6d& childArtInertia,
    const Eigen::Vector6d& childBiasForce,
    const Eigen::Vector6d& childPartialAcc)
{
  // Compute beta
  const Eigen::Vector6d beta
      = childBiasForce
        + childArtInertia
              * (childPartialAcc
                 + getRelativeJacobianStatic() * getInvProjArtInertiaImplicit()
                       * mTotalForce);
 
  //    Eigen::Vector6d beta
  //        = _childBiasForce;
  //    beta.noalias() += _childArtInertia * _childPartialAcc;
  //    beta.noalias() += _childArtInertia *  mJacobian *
  //    getInvProjArtInertiaImplicit() * mTotalForce;
 
  // Verification
  assert(!math::isNan(beta));
 
  // Add child body's bias force to parent body's bias force. Note that mT
  // should be updated.
  parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildBiasForceToKinematic(
    Eigen::Vector6d& _parentBiasForce,
    const Eigen::Matrix6d& childArtInertia,
    const Eigen::Vector6d& childBiasForce,
    const Eigen::Vector6d& childPartialAcc)
{
  // Compute beta
  const Eigen::Vector6d beta
      = childBiasForce
        + childArtInertia
              * (childPartialAcc
                 + getRelativeJacobianStatic() * getAccelerationsStatic());
 
  //    Eigen::Vector6d beta
  //        = _childBiasForce;
  //    beta.noalias() += _childArtInertia * _childPartialAcc;
  //    beta.noalias() += _childArtInertia *  mJacobian *
  //    getInvProjArtInertiaImplicit() * mTotalForce;
 
  // Verification
  assert(!math::isNan(beta));
 
  // Add child body's bias force to parent body's bias force. Note that mT
  // should be updated.
  _parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildBiasImpulseTo(
    Eigen::Vector6d& parentBiasImpulse,
    const Eigen::Matrix6d& childArtInertia,
    const Eigen::Vector6d& childBiasImpulse)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      addChildBiasImpulseToDynamic(
          parentBiasImpulse, childArtInertia, childBiasImpulse);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      addChildBiasImpulseToKinematic(
          parentBiasImpulse, childArtInertia, childBiasImpulse);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(addChildBiasImpulseTo);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildBiasImpulseToDynamic(
    Eigen::Vector6d& _parentBiasImpulse,
    const Eigen::Matrix6d& childArtInertia,
    const Eigen::Vector6d& childBiasImpulse)
{
  // Compute beta
  const Eigen::Vector6d beta = childBiasImpulse
                               + childArtInertia * getRelativeJacobianStatic()
                                     * getInvProjArtInertia() * mTotalImpulse;
 
  // Verification
  assert(!math::isNan(beta));
 
  // Add child body's bias force to parent body's bias force. Note that mT
  // should be updated.
  _parentBiasImpulse += math::dAdInvT(this->getRelativeTransform(), beta);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildBiasImpulseToKinematic(
    Eigen::Vector6d& parentBiasImpulse,
    const Eigen::Matrix6d& /*childArtInertia*/,
    const Eigen::Vector6d& childBiasImpulse)
{
  // Add child body's bias force to parent body's bias force. Note that mT
  // should be updated.
  parentBiasImpulse
      += math::dAdInvT(this->getRelativeTransform(), childBiasImpulse);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateTotalForce(
    const Eigen::Vector6d& bodyForce, double timeStep)
{
  assert(timeStep > 0.0);
 
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
      this->mAspectState.mForces = this->mAspectState.mCommands;
      updateTotalForceDynamic(bodyForce, timeStep);
      break;
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      this->mAspectState.mForces.setZero();
      updateTotalForceDynamic(bodyForce, timeStep);
      break;
    case Joint::ACCELERATION:
      setAccelerationsStatic(this->mAspectState.mCommands);
      updateTotalForceKinematic(bodyForce, timeStep);
      break;
    case Joint::VELOCITY:
      setAccelerationsStatic(
          (this->mAspectState.mCommands - getVelocitiesStatic()) / timeStep);
      updateTotalForceKinematic(bodyForce, timeStep);
      break;
    case Joint::LOCKED:
      setVelocitiesStatic(Vector::Zero());
      setAccelerationsStatic(Vector::Zero());
      updateTotalForceKinematic(bodyForce, timeStep);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateTotalForce);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateTotalForceDynamic(
    const Eigen::Vector6d& bodyForce, double timeStep)
{
  // Spring force
  const Vector springForce
      = -Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
          getPositionsStatic() - Base::mAspectProperties.mRestPositions
          + getVelocitiesStatic() * timeStep);
 
  // Damping force
  const Vector dampingForce
      = -Base::mAspectProperties.mDampingCoefficients.cwiseProduct(
          getVelocitiesStatic());
 
  //
  mTotalForce = this->mAspectState.mForces + springForce + dampingForce
                - getRelativeJacobianStatic().transpose() * bodyForce;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateTotalForceKinematic(
    const Eigen::Vector6d& /*bodyForce*/, double /*timeStep*/)
{
  // Do nothing
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateTotalImpulse(
    const Eigen::Vector6d& bodyImpulse)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      updateTotalImpulseDynamic(bodyImpulse);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      updateTotalImpulseKinematic(bodyImpulse);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateTotalImpulse);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateTotalImpulseDynamic(
    const Eigen::Vector6d& bodyImpulse)
{
  //
  mTotalImpulse = mConstraintImpulses
                  - getRelativeJacobianStatic().transpose() * bodyImpulse;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateTotalImpulseKinematic(
    const Eigen::Vector6d& /*bodyImpulse*/)
{
  // Do nothing
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::resetTotalImpulses()
{
  mTotalImpulse.setZero();
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateAcceleration(
    const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& spatialAcc)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      updateAccelerationDynamic(artInertia, spatialAcc);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      updateAccelerationKinematic(artInertia, spatialAcc);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateAcceleration);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateAccelerationDynamic(
    const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& spatialAcc)
{
  //
  setAccelerationsStatic(
      getInvProjArtInertiaImplicit()
      * (mTotalForce
         - getRelativeJacobianStatic().transpose() * artInertia
               * math::AdInvT(this->getRelativeTransform(), spatialAcc)));
 
  // Verification
  assert(!math::isNan(getAccelerationsStatic()));
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateAccelerationKinematic(
    const Eigen::Matrix6d& /*artInertia*/,
    const Eigen::Vector6d& /*spatialAcc*/)
{
  // Do nothing
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateVelocityChange(
    const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& velocityChange)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      updateVelocityChangeDynamic(artInertia, velocityChange);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      updateVelocityChangeKinematic(artInertia, velocityChange);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateVelocityChange);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateVelocityChangeDynamic(
    const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& velocityChange)
{
  //
  mVelocityChanges
      = getInvProjArtInertia()
        * (mTotalImpulse
           - getRelativeJacobianStatic().transpose() * artInertia
                 * math::AdInvT(this->getRelativeTransform(), velocityChange));
 
  // Verification
  assert(!math::isNan(mVelocityChanges));
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateVelocityChangeKinematic(
    const Eigen::Matrix6d& /*artInertia*/,
    const Eigen::Vector6d& /*velocityChange*/)
{
  // Do nothing
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateForceID(
    const Eigen::Vector6d& bodyForce,
    double timeStep,
    bool withDampingForces,
    bool withSpringForces)
{
  this->mAspectState.mForces
      = getRelativeJacobianStatic().transpose() * bodyForce;
 
  // Implicit damping force:
  //   tau_d = -Kd * dq - Kd * h * ddq
  if (withDampingForces)
  {
    const typename ConfigSpaceT::Vector dampingForces
        = -Base::mAspectProperties.mDampingCoefficients.cwiseProduct(
            getVelocitiesStatic() + getAccelerationsStatic() * timeStep);
    this->mAspectState.mForces -= dampingForces;
  }
 
  // Implicit spring force:
  //   tau_s = -Kp * (q - q0) - Kp * h * dq - Kp * h^2 * ddq
  if (withSpringForces)
  {
    const typename ConfigSpaceT::Vector springForces
        = -Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
            getPositionsStatic() - Base::mAspectProperties.mRestPositions
            + getVelocitiesStatic() * timeStep
            + getAccelerationsStatic() * timeStep * timeStep);
    this->mAspectState.mForces -= springForces;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateForceFD(
    const Eigen::Vector6d& bodyForce,
    double timeStep,
    bool withDampingForces,
    bool withSpringForces)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      updateForceID(bodyForce, timeStep, withDampingForces, withSpringForces);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateForceFD);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateImpulseID(
    const Eigen::Vector6d& bodyImpulse)
{
  mImpulses = getRelativeJacobianStatic().transpose() * bodyImpulse;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateImpulseFD(
    const Eigen::Vector6d& bodyImpulse)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      updateImpulseID(bodyImpulse);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateImpulseFD);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateConstrainedTerms(double timeStep)
{
  switch (Joint::mAspectProperties.mActuatorType)
  {
    case Joint::FORCE:
    case Joint::PASSIVE:
    case Joint::SERVO:
    case Joint::MIMIC:
      updateConstrainedTermsDynamic(timeStep);
      break;
    case Joint::ACCELERATION:
    case Joint::VELOCITY:
    case Joint::LOCKED:
      updateConstrainedTermsKinematic(timeStep);
      break;
    default:
      GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateConstrainedTerms);
      break;
  }
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateConstrainedTermsDynamic(double timeStep)
{
  const double invTimeStep = 1.0 / timeStep;
 
  setVelocitiesStatic(getVelocitiesStatic() + mVelocityChanges);
  setAccelerationsStatic(
      getAccelerationsStatic() + mVelocityChanges * invTimeStep);
  this->mAspectState.mForces.noalias() += mImpulses * invTimeStep;
  // Note: As long as this is only called from BodyNode::updateConstrainedTerms
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateConstrainedTermsKinematic(
    double timeStep)
{
  this->mAspectState.mForces.noalias() += mImpulses / timeStep;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildBiasForceForInvMassMatrix(
    Eigen::Vector6d& parentBiasForce,
    const Eigen::Matrix6d& childArtInertia,
    const Eigen::Vector6d& childBiasForce)
{
  // Compute beta
  Eigen::Vector6d beta = childBiasForce;
  beta.noalias() += childArtInertia * getRelativeJacobianStatic()
                    * getInvProjArtInertia() * mInvM_a;
 
  // Verification
  assert(!math::isNan(beta));
 
  // Add child body's bias force to parent body's bias force. Note that mT
  // should be updated.
  parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addChildBiasForceForInvAugMassMatrix(
    Eigen::Vector6d& parentBiasForce,
    const Eigen::Matrix6d& childArtInertia,
    const Eigen::Vector6d& childBiasForce)
{
  // Compute beta
  Eigen::Vector6d beta = childBiasForce;
  beta.noalias() += childArtInertia * getRelativeJacobianStatic()
                    * getInvProjArtInertiaImplicit() * mInvM_a;
 
  // Verification
  assert(!math::isNan(beta));
 
  // Add child body's bias force to parent body's bias force. Note that mT
  // should be updated.
  parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta);
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::updateTotalForceForInvMassMatrix(
    const Eigen::Vector6d& bodyForce)
{
  // Compute alpha
  mInvM_a = this->mAspectState.mForces
            - getRelativeJacobianStatic().transpose() * bodyForce;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::getInvMassMatrixSegment(
    Eigen::MatrixXd& _invMassMat,
    const size_t _col,
    const Eigen::Matrix6d& artInertia,
    const Eigen::Vector6d& spatialAcc)
{
  //
  mInvMassMatrixSegment
      = getInvProjArtInertia()
        * (mInvM_a
           - getRelativeJacobianStatic().transpose() * artInertia
                 * math::AdInvT(this->getRelativeTransform(), spatialAcc));
 
  // Verification
  assert(!math::isNan(mInvMassMatrixSegment));
 
  // Index
  size_t iStart = mDofs[0]->mIndexInTree;
 
  // Assign
  _invMassMat.block<NumDofs, 1>(iStart, _col) = mInvMassMatrixSegment;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::getInvAugMassMatrixSegment(
    Eigen::MatrixXd& invMassMat,
    const size_t col,
    const Eigen::Matrix6d& artInertia,
    const Eigen::Vector6d& spatialAcc)
{
  //
  mInvMassMatrixSegment
      = getInvProjArtInertiaImplicit()
        * (mInvM_a
           - getRelativeJacobianStatic().transpose() * artInertia
                 * math::AdInvT(this->getRelativeTransform(), spatialAcc));
 
  // Verification
  assert(!math::isNan(mInvMassMatrixSegment));
 
  // Index
  size_t iStart = mDofs[0]->mIndexInTree;
 
  // Assign
  invMassMat.block<NumDofs, 1>(iStart, col) = mInvMassMatrixSegment;
}
 
//==============================================================================
template <class ConfigSpaceT>
void GenericJoint<ConfigSpaceT>::addInvMassMatrixSegmentTo(Eigen::Vector6d& acc)
{
  //
  acc += getRelativeJacobianStatic() * mInvMassMatrixSegment;
}
 
//==============================================================================
template <class ConfigSpaceT>
Eigen::VectorXd GenericJoint<ConfigSpaceT>::getSpatialToGeneralized(
    const Eigen::Vector6d& spatial)
{
  return getRelativeJacobianStatic().transpose() * spatial;
}
 
} // namespace dynamics
} // namespace dart
 
#endif // DART_DYNAMICS_DETAIL_GenericJoint_HPP_