Merge pull request #168 from dartsim/issue122

Fix incorrect integration of BallJoint and FreeJoint using modified integration API

Changelog.md

@@ -2,22 +2,25 @@
 
 ### Version 4.0.0 (2014-0X-XX)
 
-1. Fixed self collision bug
-  * [Issue #125](https://github.com/dartsim/dart/issues/125)
 1. Improved collision handling using Featherstone algorithm
   * [Issue #85](https://github.com/dartsim/dart/issues/87)
 1. Added implicit joint spring force and damping force
 1. Added planar joint
 1. Added initial implementation of soft body dynamics
 1. Added useful kinematical properties pertain to COM of skeleton
-1. Improved optimizer interface and added nlopt solver
-  * [Pull request #152](https://github.com/dartsim/dart/pull/152)
 1. Added initial implementation of inverse kinematics functions
   * [Pull request #154](https://github.com/dartsim/dart/pull/154)
 1. Added optional collision detector: BulletCollision.
   * [Pull request #156](https://github.com/dartsim/dart/pull/156)
 1. Improved kinematics API for skeleton and joint
   * [Pull request #161](https://github.com/dartsim/dart/pull/161)
+1. Improved optimizer interface and added nlopt solver
+  * [Pull request #152](https://github.com/dartsim/dart/pull/152)
+1. Fixed self collision bug
+  * [Issue #125](https://github.com/dartsim/dart/issues/125)
+1. Fixed incorrect integration of BallJoint and FreeJoint
+  * [Issue #122](https://github.com/dartsim/dart/issues/122)
+  * [Pull request #168](https://github.com/dartsim/dart/pull/168)
 
 ### Version 3.0 (2013-11-04)
 

Migration.md

@@ -8,6 +8,14 @@
   + bool isInf(double _v)
   + bool isInf(const Eigen::MatrixXd& _m)
 
+1. **dart/dynamics/GenCoord.h**
+  + void integrateConfig(double _dt)
+  + void integrateVel(double _dt)
+
+1. **dart/dynamics/GenCoordSystem**
+  + virtual void integrateConfigs(double _dt)
+  + virtual void integrateGenVels(double _dt)
+
 1. **dart/dynamics/BodyNode.h**
   + enum InverseKinematicsPolicy
   + class TransformObjFunc
@@ -27,13 +35,40 @@
   + virtual void setGenAcc(size_t _idx, double _genAcc, bool _updateAccs = true)
   + virtual void setGenAccs(const Eigen::VectorXd& _genAccs, bool _updateAccs = true)
 
+1. **dart/dynamics/BallJoint.h**
+  + virtual void setTransformFromChildBodyNode(const Eigen::Isometry3d& _T)
+  + virtual void integrateConfigs(double _dt)
+
+1. **dart/dynamics/FreeJoint.h**
+  + virtual void setTransformFromChildBodyNode(const Eigen::Isometry3d& _T)
+  + virtual void integrateConfigs(double _dt)
+
 1. **dart/dynamics/Skeleton.h**
   + virtual void setGenVels(const Eigen::VectorXd& _genVels, bool _updateVels = true, bool _updateAccs = true)
   + virtual void setGenAccs(const Eigen::VectorXd& _genAccs, bool _updateAccs = true)
+  + virtual void integrateConfigs(double _dt)
+  + virtual void integrateGenVels(double _dt)
   + void computeForwardKinematics(bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
 
+1. **dart/simulation/World.h**
+  + virtual void integrateConfigs(const Eigen::VectorXd& _genVels, double _dt)
+  + virtual void integrateGenVels(const Eigen::VectorXd& _genAccs, double _dt)
+
+1. **dart/integration/Integrator.h**
+  + virtual void integrateConfigs(const Eigen::VectorXd& _genVels, double _dt) = 0
+  + virtual void integrateGenVels(const Eigen::VectorXd& _genAccs, double _dt) = 0
+
 ### Deletions
 
+1. **dart/dynamics/BodyNode.h**
+  + virtual void updateTransform_Issue122(double _timeStep)
+  + virtual void updateEta_Issue122()
+
+1. **dart/dynamics/Joint.h**
+  + virtual void updateTransform_Issue122(double _timeStep)
+  + virtual void updateEta_Issue122()
+  + virtual void updateJacobianTimeDeriv_Issue122()
+
 1. **dart/dynamics/Skeleton.h**
   + Eigen::VectorXd getConfig() const
 
@@ -43,27 +78,55 @@
 
 ### Modifications
 
-1. **dart/dynamics/Skeleton.h**
+1. **dart/dynamics/BodyNode.h**
+  + ***From:*** int getNumContactForces() const
+    + ***To:*** int getNumContacts() const
 
-  + ***From:*** void setConfig(const std::vector<int>& _id, const Eigen::VectorXd& _config)
-  + ***To:*** void setConfigSegs(const std::vector<int>& _id, const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
+1. **dart/dynamics/Joint.h**
+  + ***From:*** void setTransformFromParentBodyNode(const Eigen::Isometry3d& _T)
+    + ***To:*** virtual void setTransformFromParentBodyNode(const Eigen::Isometry3d& _T)
 
+1. **dart/dynamics/Skeleton.h**
+  + ***From:*** void setConfig(const std::vector<int>& _id, const Eigen::VectorXd& _config)
+    + ***To:*** void setConfigSegs(const std::vector<int>& _id, const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
   + ***From:*** Eigen::VectorXd getConfig(const std::vector<int>& _id) const
-  + ***To:*** Eigen::VectorXd getConfigSegs(const std::vector<int>& _id) const
-
+    + ***To:*** Eigen::VectorXd getConfigSegs(const std::vector<int>& _id) const
   + ***From:*** void setConfig(const Eigen::VectorXd& _config)
-  + ***To:*** virtual void setConfigs(const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
-
+    + ***To:*** virtual void setConfigs(const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
   + ***From:*** void setState(const Eigen::VectorXd& _state)
-  + ***To:*** void setState(const Eigen::VectorXd& _state, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
-
+    + ***To:*** void setState(const Eigen::VectorXd& _state, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true)
   + ***From:*** Eigen::VectorXd getState()
-  + ***To:*** Eigen::VectorXd getState() const
-
-1. **dart/dynamics/BodyNode.h**
-
-  + ***From:*** int getNumContactForces() const
-  + ***To:*** int getNumContacts() const
+    + ***To:*** Eigen::VectorXd getState() const
+
+1. **dart/simulation/World.h**
+  + ***From:*** virtual void setState(const Eigen::VectorXd &_newState)
+    + ***To:*** virtual void setConfigs(const Eigen::VectorXd& _configs)
+    + ***To:*** virtual void setGenVels(const Eigen::VectorXd& _genVels)
+  + ***From:*** virtual Eigen::VectorXd getState() const
+    + ***To:*** virtual Eigen::VectorXd getConfigs() const
+    + ***To:*** virtual Eigen::VectorXd getGenVels() const
+  + ***From:*** virtual Eigen::VectorXd evalDeriv()
+    + ***To:*** virtual Eigen::VectorXd evalGenAccs()
+
+1. **dart/integration/Integrator.h**
+  + ***From:*** virtual void setState(const Eigen::VectorXd &_newState) = 0
+    + ***To:*** virtual void setConfigs(const Eigen::VectorXd& _configs) = 0
+    + ***To:*** virtual void setGenVels(const Eigen::VectorXd& _genVels) = 0
+  + ***From:*** virtual Eigen::VectorXd getState() const = 0
+    + ***To:*** virtual Eigen::VectorXd getConfigs() const = 0
+    + ***To:*** virtual Eigen::VectorXd getGenVels() const = 0
+  + ***From:*** virtual Eigen::VectorXd evalDeriv() = 0
+    + ***To:*** virtual Eigen::VectorXd evalGenAccs() = 0
+  + ***From:*** virtual void integrate(IntegrableSystem* system, double dt) const = 0
+    + ***To:*** virtual void integrate(IntegrableSystem* system, double dt) = 0
+
+1. **dart/integration/EulerIntegrator.h**
+  + ***From:*** virtual void integrate(IntegrableSystem* system, double dt) const = 0
+    + ***To:*** virtual void integrate(IntegrableSystem* system, double dt) = 0
+
+1. **dart/integration/RK4Integrator.h**
+  + ***From:*** virtual void integrate(IntegrableSystem* system, double dt) const = 0
+    + ***To:*** virtual void integrate(IntegrableSystem* system, double dt) = 0
 
 ### New Deprecations
 

apps/atlasRobot/Controller.cpp

@@ -347,7 +347,6 @@ void Controller::unharnessRightFoot()
 void Controller::resetRobot()
 {
   int dof = mAtlasRobot->getNumGenCoords();
-  mAtlasRobot->setConfigs(mInitialState.head(dof), true, true, false);  // See #122
   mAtlasRobot->setState(mInitialState, true, true, false);
 
   dtmsg << "Robot is reset." << std::endl;

dart/dynamics/BallJoint.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Georgia Tech Research Corporation
+ * Copyright (c) 2013-2014, Georgia Tech Research Corporation
  * All rights reserved.
  *
  * Author(s): Jeongseok Lee <jslee02@gmail.com>
@@ -44,75 +44,47 @@
 namespace dart {
 namespace dynamics {
 
+//==============================================================================
 BallJoint::BallJoint(const std::string& _name)
   : Joint(_name),
-    mT_Joint(Eigen::Isometry3d::Identity())
+    mR(Eigen::Isometry3d::Identity())
 {
   mGenCoords.push_back(&mCoordinate[0]);
   mGenCoords.push_back(&mCoordinate[1]);
   mGenCoords.push_back(&mCoordinate[2]);
 
   mS = Eigen::Matrix<double, 6, 3>::Zero();
+  Eigen::Vector6d J0 = Eigen::Vector6d::Zero();
+  Eigen::Vector6d J1 = Eigen::Vector6d::Zero();
+  Eigen::Vector6d J2 = Eigen::Vector6d::Zero();
+  J0[0] = 1.0;
+  J1[1] = 1.0;
+  J2[2] = 1.0;
+  mS.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
+  mS.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
+  mS.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
+  assert(!math::isNan(mS));
+
   mdS = Eigen::Matrix<double, 6, 3>::Zero();
 
   mSpringStiffness.resize(3, 0.0);
   mDampingCoefficient.resize(3, 0.0);
   mRestPosition.resize(3, 0.0);
 }
 
-BallJoint::~BallJoint() {
-}
-
-inline void BallJoint::updateTransform() {
-  Eigen::Vector3d q(mCoordinate[0].getConfig(),
-                    mCoordinate[1].getConfig(),
-                    mCoordinate[2].getConfig());
-
-  // TODO(JS): This is workaround for Issue #122.
-  mT_Joint = math::expAngular(q);
-
-  mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
-
-  assert(math::verifyTransform(mT));
-}
-
-void BallJoint::updateTransform_Issue122(double _timeStep) {
-  mT_Joint = mT_Joint * math::expAngular(_timeStep * getGenVels());
-
-  GenCoordSystem::setConfigs(math::logMap(mT_Joint).head<3>());
-
-  mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse();
-
-  assert(math::verifyTransform(mT));
+//==============================================================================
+BallJoint::~BallJoint()
+{
 }
 
-inline void BallJoint::updateJacobian() {
-  Eigen::Vector3d q(mCoordinate[0].getConfig(),
-                    mCoordinate[1].getConfig(),
-                    mCoordinate[2].getConfig());
-
-  Eigen::Matrix3d J = math::expMapJac(q);
-
-  Eigen::Vector6d J0;
-  Eigen::Vector6d J1;
-  Eigen::Vector6d J2;
-
-  J0 << J(0, 0), J(0, 1), J(0, 2), 0, 0, 0;
-  J1 << J(1, 0), J(1, 1), J(1, 2), 0, 0, 0;
-  J2 << J(2, 0), J(2, 1), J(2, 2), 0, 0, 0;
-
-  mS.col(0) = math::AdT(mT_ChildBodyToJoint, J0);
-  mS.col(1) = math::AdT(mT_ChildBodyToJoint, J1);
-  mS.col(2) = math::AdT(mT_ChildBodyToJoint, J2);
-
-  assert(!math::isNan(mS));
-}
+//==============================================================================
+void BallJoint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T)
+{
+  Joint::setTransformFromChildBodyNode(_T);
 
-void BallJoint::updateJacobian_Issue122() {
   Eigen::Vector6d J0 = Eigen::Vector6d::Zero();
   Eigen::Vector6d J1 = Eigen::Vector6d::Zero();
   Eigen::Vector6d J2 = Eigen::Vector6d::Zero();
-
   J0[0] = 1.0;
   J1[1] = 1.0;
   J2[2] = 1.0;
@@ -124,36 +96,37 @@ void BallJoint::updateJacobian_Issue122() {
   assert(!math::isNan(mS));
 }
 
-inline void BallJoint::updateJacobianTimeDeriv() {
-  Eigen::Vector3d q(mCoordinate[0].getConfig(),
-      mCoordinate[1].getConfig(),
-      mCoordinate[2].getConfig());
-  Eigen::Vector3d dq(mCoordinate[0].getVel(),
-      mCoordinate[1].getVel(),
-      mCoordinate[2].getVel());
+//==============================================================================
+void BallJoint::integrateConfigs(double _dt)
+{
+  mR.linear() = mR.linear() * math::expMapRot(getGenVels() * _dt);
 
-  Eigen::Matrix3d dJ = math::expMapJacDot(q, dq);
+  GenCoordSystem::setConfigs(math::logMap(mR.linear()));
+}
 
-  Eigen::Vector6d dJ0;
-  Eigen::Vector6d dJ1;
-  Eigen::Vector6d dJ2;
+//==============================================================================
+void BallJoint::updateTransform()
+{
+  mR.linear() = math::expMapRot(getConfigs());
 
-  dJ0 << dJ(0, 0), dJ(0, 1), dJ(0, 2), 0, 0, 0;
-  dJ1 << dJ(1, 0), dJ(1, 1), dJ(1, 2), 0, 0, 0;
-  dJ2 << dJ(2, 0), dJ(2, 1), dJ(2, 2), 0, 0, 0;
+  mT = mT_ParentBodyToJoint * mR * mT_ChildBodyToJoint.inverse();
 
-  mdS.col(0) = math::AdT(mT_ChildBodyToJoint, dJ0);
-  mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1);
-  mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2);
+  assert(math::verifyTransform(mT));
+}
 
-  assert(!math::isNan(mdS));
+//==============================================================================
+void BallJoint::updateJacobian()
+{
+  // Jacobian is constant
 }
 
-void BallJoint::updateJacobianTimeDeriv_Issue122() {
-  // mdS == 0
-  mdS.setZero();
-//  assert(mdS == Eigen::MatrixXd::Zero(6, 3));
+//==============================================================================
+void BallJoint::updateJacobianTimeDeriv()
+{
+  // Time derivative of Jacobian is constant
+  assert(mdS == Eigen::MatrixXd::Zero(6, 3));
 }
 
 }  // namespace dynamics
 }  // namespace dart
+

dart/dynamics/BallJoint.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013, Georgia Tech Research Corporation
+ * Copyright (c) 2013-2014, Georgia Tech Research Corporation
  * All rights reserved.
  *
  * Author(s): Jeongseok Lee <jslee02@gmail.com>
@@ -47,42 +47,41 @@
 namespace dart {
 namespace dynamics {
 
-class BallJoint : public Joint {
+/// \brief class BallJoint
+class BallJoint : public Joint
+{
 public:
-  /// \brief Destructor.
-  explicit BallJoint(const std::string& _name = "Noname BallJoint");
+  /// \brief Constructor
+  explicit BallJoint(const std::string& _name = "BallJoint");
 
-  /// \brief Constructor.
+  /// \brief Destructor
   virtual ~BallJoint();
 
-  // Documentation inherited.
-  virtual void updateTransform();
+  // Documentation inherited
+  virtual void setTransformFromChildBodyNode(const Eigen::Isometry3d& _T);
 
-  /// @brief TODO(JS): This is workaround for Issue #122.
-  virtual void updateTransform_Issue122(double _timeStep);
+protected:
+  // Documentation inherited
+  virtual void integrateConfigs(double _dt);
 
-  // Documentation inherited.
-  virtual void updateJacobian();
+  // Documentation inherited
+  virtual void updateTransform();
 
-  /// @brief TODO(JS): This is workaround for Issue #122.
-  virtual void updateJacobian_Issue122();
+  // Documentation inherited
+  virtual void updateJacobian();
 
-  // Documentation inherited.
+  // Documentation inherited
   virtual void updateJacobianTimeDeriv();
 
-  /// @brief TODO(JS): This is workaround for Issue #122.
-  virtual void updateJacobianTimeDeriv_Issue122();
-
 protected:
-  /// \brief
+  /// \brief Generalized coordinates
   GenCoord mCoordinate[3];
 
-  /// @brief
-  // TODO(JS): This is workaround for Issue #122.
-  Eigen::Isometry3d mT_Joint;
+  /// \brief Rotation matrix
+  Eigen::Isometry3d mR;
 
 public:
-  //
+  // To get byte-aligned Eigen vectors
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };