Merge my project: Real time iLQR control and SysID code into main branch

dart/dynamics/Skeleton.cpp

@@ -3159,7 +3159,7 @@ Eigen::VectorXs Skeleton::getLinkMOIs()
   return inertias;
 }
 
-// TODO: Check The Sign of MOI
+//==============================================================================
 Eigen::Matrix3s Skeleton::getMOIMatrix(Eigen::Vector6s moi_vector)
 {
   Eigen::Matrix3s MOI = moi_vector.segment(0,3).asDiagonal();
@@ -3172,6 +3172,7 @@ Eigen::Matrix3s Skeleton::getMOIMatrix(Eigen::Vector6s moi_vector)
   return MOI;
 }
 
+//==============================================================================
 Eigen::Vector6s Skeleton::getLinkMOIIndex(size_t index)
 {
   Eigen::Vector6s inertia = Eigen::Vector6s::Zero();

dart/dynamics/Skeleton.hpp

@@ -862,21 +862,29 @@ class Skeleton : public virtual common::VersionCounter,
   // This get particular moment of inertia of a body node
   Eigen::Vector6s getLinkMOIIndex(size_t index);
 
+  // This get all body nodes' moment of inertia in a matrix
   Eigen::Matrix3s getMOIMatrix(Eigen::Vector6s moi_vector);
 
   // This returns a vector of all the link masses for all the links in this
   // skeleton concatenated into a flat vector.
   Eigen::VectorXs getLinkMasses();
 
-  // This returns a matrix of A_k as describe in Karen's paper
+  // This returns a matrix of A_k as describe in paper
+  // https://arxiv.org/pdf/1907.03964.pdf
+  // You can see detailed description of this matrix in the last appendix
+  // section of this paper.
   Eigen::MatrixXs getLinkAkMatrixIndex(size_t index);
 
+  // This returns a jacobian matrix of velocity connect between world and joint coordinate
   Eigen::MatrixXs getLinkJvkMatrixIndex(size_t index);
 
+  // This returns a jacobian matrix of angular velocity connect between world and joint coordinate
   Eigen::MatrixXs getLinkJwkMatrixIndex(size_t index);
 
+  // This returns a rotation matrix of body node wrt to world coordinate
   Eigen::Matrix3s getLinkRMatrixIndex(size_t index);
 
+  // This returns a jacobian matrix of angular velocity connect between world and joint coordiante
   Eigen::MatrixXs getLinkLocalJwkMatrixIndex(size_t index);
 
   // Sets the upper limits of all the joints from a single vector
@@ -905,20 +913,26 @@ class Skeleton : public virtual common::VersionCounter,
 
   void setLinkBetas(Eigen::VectorXs betas);
 
-  // This sets all the inertia center-of-mass vectors for all the links in this
+  // This sets all the COM vectors for all the links in this
   // skeleton concatenated together
   void setLinkCOMs(Eigen::VectorXs coms);
 
+  // This set COM of a particular body node specified by index. 
   void setLinkCOMIndex(Eigen::Vector3s com, size_t index);
 
-  void setLinkDiagIs(Eigen::VectorXs coms);
+  // This set all the diagonal term of moment of inertias.
+  void setLinkDiagIs(Eigen::VectorXs mois);
 
+  // This set diagonal term of moment of inertia of a particular 
+  // body node specified by index 
   void setLinkDiagIIndex(Eigen::Vector3s com, size_t index);
 
   // This sets all the inertia moment-of-inertia paremeters for all the links in
   // this skeleton concatenated together
   void setLinkMOIs(Eigen::VectorXs mois);
 
+  // This set moment of inertia of a particular body node specified
+  // by index.
   void setLinkMOIIndex(Eigen::Vector6s moi, size_t index);
 
   // This returns a vector of all the link masses for all the links in this

dart/neural/BackpropSnapshot.cpp

@@ -1027,89 +1027,6 @@ const Eigen::MatrixXs& BackpropSnapshot::getContactFreeVelVelJacobian(
   return mCachedContactFreeVelVel;
 }
 
-//==============================================================================
-/*
-const Eigen::MatrixXs& BackpropSnapshot::getContactReducedVelVelJacobian(
-  WorldPtr world, Eigen::VectorXs index, PerformanceLog* perfLog)
-{
-  #ifndef NDEBUG
-  assert(
-      world->getPositions() == mPreStepPosition
-      && world->getVelocities() == mPreStepVelocity);
-#endif
-
-  PerformanceLog* thisLog = nullptr;
-#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT
-  if (perfLog != nullptr)
-  {
-    thisLog = perfLog->startRun("BackpropSnapshot.getVelVelJacobian");
-  }
-#endif
-
-  if (mCachedVelVelDirty)
-  {
-    PerformanceLog* refreshLog = nullptr;
-#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT
-    if (thisLog != nullptr)
-    {
-      refreshLog = thisLog->startRun(
-          "BackpropSnapshot.getVelVelJacobian#refreshCache");
-    }
-#endif
-
-    if (mUseFDOverride)
-    {
-      mCachedVelVel = finiteDifferenceVelVelJacobian(world);
-    }
-    else
-    {
-      Eigen::VectorXs ddamp = getDampingVector(world);
-      Eigen::VectorXs spring_stiffs = getSpringStiffVector(world);
-      Eigen::MatrixXs Minv = getInvMassMatrix(world);
-      s_t dt = world->getTimeStep();
-      Eigen::MatrixXs A_c = getClampingConstraintMatrix(world);
-
-      // If there are no clamping constraints, then vel-vel is just the identity
-      if (A_c.size() == 0)
-      {
-        mCachedVelVel = Eigen::MatrixXs::Identity(mNumDOFs, mNumDOFs)
-                        - dt * Minv * ddamp.asDiagonal()
-                        - dt * dt * Minv * spring_stiffs.asDiagonal()
-                        - dt * Minv * getVelCJacobian(world);
-      }
-      else
-      {
-
-        mCachedVelVel = getVelJacobianWrt(world, WithRespectTo::VELOCITY)
-                        - dt * Minv * ddamp.asDiagonal()
-                        - dt * dt * Minv * spring_stiffs.asDiagonal();
-      }
-    }
-
-    if (mSlowDebugResultsAgainstFD)
-    {
-      Eigen::MatrixXs bruteForce = finiteDifferenceVelVelJacobian(world);
-      equalsOrCrash(world, mCachedVelVel, bruteForce, "vel-vel");
-    }
-
-    mCachedVelVelDirty = false;
-#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT
-    if (refreshLog != nullptr)
-    {
-      refreshLog->end();
-    }
-#endif
-  }
-
-#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT
-  if (thisLog != nullptr)
-  {
-    thisLog->end();
-  }
-#endif
-  return mCachedVelVel;
-}
-*/
 //==============================================================================
 const Eigen::MatrixXs& BackpropSnapshot::getPosVelJacobian(
     WorldPtr world, PerformanceLog* perfLog)

dart/neural/BackpropSnapshot.hpp

@@ -70,37 +70,39 @@ class BackpropSnapshot
   const Eigen::MatrixXs& getVelVelJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
+  /// This computes and returns the whole vel-vel jacobian. For backprop, you
+  /// don't actually need this matrix, you can compute backprop directly. This
+  /// is here if you want access to the full Jacobian for some reason.
+  /// This assume all contact constraints are separating.
   const Eigen::MatrixXs& getContactFreeVelVelJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
-  /// Index should specify which contact will be kept
-  const Eigen::MatrixXs& getContactReducedVelVelJacobian(
-      simulation::WorldPtr world, Eigen::VectorXs indexs, PerformanceLog* perfLog = nullptr);
-
   /// This computes and returns the whole pos-vel jacobian. For backprop, you
   /// don't actually need this matrix, you can compute backprop directly. This
   /// is here if you want access to the full Jacobian for some reason.
   const Eigen::MatrixXs& getPosVelJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
+  /// This computes and returns the whole pos-vel jacobian. For backprop, you
+  /// don't actually need this matrix, you can compute backprop directly. This
+  /// is here if you want access to the full Jacobian for some reason.
+  /// This assume all contact constraints are separating.
   const Eigen::MatrixXs& getContactFreePosVelJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
-  const Eigen::MatrixXs& getContactReducedPosVelJacobian(
-      simulation::WorldPtr world, Eigen::VectorXs indexs, PerformanceLog* perfLog = nullptr);
-
   /// This computes and returns the whole force-vel jacobian. For backprop, you
   /// don't actually need this matrix, you can compute backprop directly. This
   /// is here if you want access to the full Jacobian for some reason.
   const Eigen::MatrixXs& getControlForceVelJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
+  /// This computes and returns the whole force-vel jacobian. For backprop, you
+  /// don't actually need this matrix, you can compute backprop directly. This
+  /// is here if you want access to the full Jacobian for some reason.
+  /// This assume all contact constraints are separating.
   const Eigen::MatrixXs& getContactFreeControlForceVelJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
-  const Eigen::MatrixXs& getContactReducedCControlForceVelJacobian(
-      simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
-
   /// This computes and returns the whole mass-vel jacobian. For backprop, you
   /// don't actually need this matrix, you can compute backprop directly. This
   /// is here if you want access to the full Jacobian for some reason.
@@ -125,6 +127,10 @@ class BackpropSnapshot
   const Eigen::MatrixXs& getPosPosJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
+  /// This computes and returns the whole pos-pos jacobian. For backprop, you
+  /// don't actually need this matrix, you can compute backprop directly. This
+  /// is here if you want access to the full Jacobian for some reason.
+  /// This assume all contact constraints are separating.
   const Eigen::MatrixXs& getContactFreePosPosJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
@@ -134,6 +140,11 @@ class BackpropSnapshot
   const Eigen::MatrixXs& getVelPosJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
+
+  /// This computes and returns the whole pos-pos jacobian. For backprop, you
+  /// don't actually need this matrix, you can compute backprop directly. This
+  /// is here if you want access to the full Jacobian for some reason.
+  /// This assume all contact constraints are separating
   const Eigen::MatrixXs& getContactFreeVelPosJacobian(
       simulation::WorldPtr world, PerformanceLog* perfLog = nullptr);
 
@@ -147,20 +158,17 @@ class BackpropSnapshot
   /// This returns the Jacobian for state_t -> state_{t+1}.
   Eigen::MatrixXs getStateJacobian(simulation::WorldPtr world);
 
+  /// This returns the Jacobian for state_t -> state_{t+1}
+  /// This assume all contact constraints are separating.
   Eigen::MatrixXs getContactFreeStateJacobian(simulation::WorldPtr world);
 
-  Eigen::MatrixXs getContactReducedStateJacobian(
-      simulation::WorldPtr world,
-      Eigen::VectorXs indexs);
-
   /// This returns the Jacobian for action_t -> state_{t+1}.
   Eigen::MatrixXs getActionJacobian(simulation::WorldPtr world);
-
+
+  /// This returns the Jacobian for action_t -> state_{t+1}.
+  /// This assume all contact constraints are separating.
   Eigen::MatrixXs getContactFreeActionJacobian(simulation::WorldPtr world);
 
-  Eigen::MatrixXs getContactReducedActionJacobian(
-      simulation::WorldPtr world,
-      Eigen::VectorXs indexs);
   /// Returns a concatenated vector of all the Skeletons' position()'s in the
   /// World, in order in which the Skeletons appear in the World's
   /// getSkeleton(i) returns them, BEFORE the timestep.
@@ -347,12 +355,6 @@ class BackpropSnapshot
   Eigen::MatrixXs getVelJacobianWrt(
       simulation::WorldPtr world, WithRespectTo* wrt);
 
-  /// This computes and returns the whole wrt-vel jacobian. For backprop, you
-  /// don't actually need this matrix, you can compute backprop directly. This
-  /// is here if you want access to the full Jacobian for some reason
-  Eigen::MatrixXs getContactReducedVelJacobianWrt(
-      simulation::WorldPtr world, WithRespectTo* wrt, Eigen::VectorXs index);
-
   /// This computes and returns the whole wrt-pos jacobian. For backprop, you
   /// don't actually need this matrix, you can compute backprop directly. This
   /// is here if you want access to the full Jacobian for some reason.

dart/realtime/RealTimeControlBuffer.hpp

@@ -130,16 +130,16 @@ class RealTimeControlBuffer
   /// This is the B buffer of forces
   Eigen::MatrixXs mBufB;
 
-  // This is the A buffer of k
+  // This is the A buffer of k, which is feed forward gain in iLQR
   Eigen::MatrixXs mkBufA;
 
-  // This is the B buffer of k
+  // This is the B buffer of k, which is feed forward gain in iLQR
   Eigen::MatrixXs mkBufB;
 
-  // This is the A buffer of K
+  // This is the A buffer of K, which is feedback gain in iLQR
   std::vector<Eigen::MatrixXs> mKBufA;
 
-  // This is the B buffer of K
+  // This is the B buffer of K, which is feedback gain in iLQR
   std::vector<Eigen::MatrixXs> mKBufB;
 
   // This is the A buffer of State
@@ -150,10 +150,10 @@ class RealTimeControlBuffer
 
   Eigen::MatrixXs mxBufB;
 
-  // This is the A buffer of Alpha
+  // This is the A buffer of Alpha, which is line search learning rate in iLQR
   Eigen::VectorXs mAlphaBufA;
 
-  // This is the B buffer of Alpha
+  // This is the B buffer of Alpha, , which is line search learning rate in iLQR
   Eigen::VectorXs mAlphaBufB;
 
   /// This is the time when the last buffer was written to

unittests/comprehensive/CMakeLists.txt

@@ -114,6 +114,8 @@ if(TARGET dart-utils)
     target_link_libraries(test_cartpoleRealtime dart-utils-urdf)
     target_link_libraries(test_iLQRRealtime dart-utils)
     target_link_libraries(test_iLQRRealtime dart-utils-urdf)
+    target_link_libraries(test_iLQR dart-utils)
+    target_link_libraries(test_iLQR dart-utils-urdf)
 
     dart_add_test("comprehensive" test_Gradients)
     target_link_libraries(test_Gradients dart-utils-urdf)