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

dart/dynamics/Skeleton.cpp

@@ -2117,12 +2117,13 @@ Eigen::MatrixXs Skeleton::getUnconstrainedVelJacobianWrt(
 
   Eigen::MatrixXs Minv = getInvMassMatrix();
   Eigen::MatrixXs dC = getJacobianOfC(wrt);
+  Eigen::VectorXs spring_stiffs = getSpringStiffVector();
 
   if (wrt == neural::WithRespectTo::POSITION)
   {
     Eigen::MatrixXs dM = getJacobianOfMinv(
         dt * (tau - C - getDampingForce() - getSpringForce()), wrt);
-    return dM - Minv * dt * dC;
+    return dM - Minv * dt * dC - Minv * dt *  spring_stiffs.asDiagonal();
   }
   else
   {
@@ -3028,6 +3029,11 @@ std::size_t Skeleton::getLinkCOMDims()
   return 3 * getNumBodyNodes();
 }
 
+std::size_t Skeleton::getLinkDiagIDims()
+{
+  return 3 * getNumBodyNodes();
+}
+
 //==============================================================================
 std::size_t Skeleton::getLinkMOIDims()
 {
@@ -3109,6 +3115,32 @@ Eigen::Vector3s Skeleton::getLinkCOMIndex(size_t index)
   return mass_center;
 }
 
+Eigen::VectorXs Skeleton::getLinkDiagIs()
+{
+  Eigen::VectorXs diag_Is = Eigen::VectorXs::Zero(getLinkDiagIDims());
+  std::size_t cursor = 0;
+  for(std::size_t i = 0; i < getNumBodyNodes(); i++)
+  {
+    const Inertia& inertia=  getBodyNode(i)->getInertia();
+    s_t mass = getBodyNode(i)->getMass();
+    diag_Is(cursor++) = inertia.getParameter(dynamics::Inertia::Param::I_XX) / mass;
+    diag_Is(cursor++) = inertia.getParameter(dynamics::Inertia::Param::I_YY) / mass;
+    diag_Is(cursor++) = inertia.getParameter(dynamics::Inertia::Param::I_ZZ) / mass;
+  }
+  return diag_Is;
+}
+
+Eigen::Vector3s Skeleton::getLinkDiagIIndex(size_t index)
+{
+  Eigen::Vector3s diag_I = Eigen::Vector3s::Zero();
+  const Inertia& node_inertia = getBodyNode(index)->getInertia();
+  s_t mass = getBodyNode(index)->getMass();
+  diag_I(0) = node_inertia.getParameter(dynamics::Inertia::Param::I_XX) / mass;
+  diag_I(1) = node_inertia.getParameter(dynamics::Inertia::Param::I_YY) / mass;
+  diag_I(2) = node_inertia.getParameter(dynamics::Inertia::Param::I_ZZ) / mass;
+  return diag_I;
+}
+
 //==============================================================================
 Eigen::VectorXs Skeleton::getLinkMOIs()
 {
@@ -3127,6 +3159,20 @@ Eigen::VectorXs Skeleton::getLinkMOIs()
   return inertias;
 }
 
+//==============================================================================
+Eigen::Matrix3s Skeleton::getMOIMatrix(Eigen::Vector6s moi_vector)
+{
+  Eigen::Matrix3s MOI = moi_vector.segment(0,3).asDiagonal();
+  MOI(1,0) = moi_vector(3);
+  MOI(0,1) = moi_vector(3);
+  MOI(2,0) = moi_vector(4);
+  MOI(0,2) = moi_vector(4);
+  MOI(2,1) = moi_vector(5);
+  MOI(1,2) = moi_vector(5);
+  return MOI;
+}
+
+//==============================================================================
 Eigen::Vector6s Skeleton::getLinkMOIIndex(size_t index)
 {
   Eigen::Vector6s inertia = Eigen::Vector6s::Zero();
@@ -3151,6 +3197,43 @@ Eigen::VectorXs Skeleton::getLinkMasses()
   return masses;
 }
 
+//==============================================================================
+Eigen::MatrixXs Skeleton::getLinkAkMatrixIndex(size_t index)
+{
+  Eigen::MatrixXs J = getWorldJacobian(getBodyNode(index)); // TODO: May be problematic
+  Eigen::MatrixXs Jv = J.block(0, 0, 3, J.cols());
+  Eigen::MatrixXs Jw = J.block(3, 0, 3, J.cols());
+  Eigen::MatrixXs I = getMOIMatrix(getLinkMOIIndex(index));
+  Eigen::MatrixXs A_k = Jv.transpose() * Jv + Jw.transpose() * (I/getBodyNode(index)->getMass()) * Jw;
+  return A_k;
+}
+
+Eigen::MatrixXs Skeleton::getLinkJvkMatrixIndex(size_t index)
+{
+  Eigen::MatrixXs J = getWorldJacobian(getBodyNode(index)); // TODO: May be problematic
+  Eigen::MatrixXs Jv = J.block(0, 0, 3, J.cols()); // 3 * N matrix
+  return Jv;
+}
+
+Eigen::MatrixXs Skeleton::getLinkJwkMatrixIndex(size_t index)
+{
+  Eigen::MatrixXs J = getWorldJacobian(getBodyNode(index));
+  Eigen::MatrixXs Jw = J.block(3, 0, 3, J.cols());
+  return Jw;
+}
+
+Eigen::Matrix3s Skeleton::getLinkRMatrixIndex(size_t index)
+{
+  return getBodyNode(index)->getWorldTransform().linear();
+}
+
+Eigen::MatrixXs Skeleton::getLinkLocalJwkMatrixIndex(size_t index)
+{
+  Eigen::MatrixXs J = getJacobian(getBodyNode(index));
+  Eigen::MatrixXs Jw = J.block(3, 0, 3, J.cols());
+  return Jw;
+}
+
 //==============================================================================
 void Skeleton::setControlForceUpperLimits(Eigen::VectorXs limits)
 {
@@ -3292,6 +3375,53 @@ void Skeleton::setLinkCOMIndex(Eigen::Vector3s com, size_t index)
   getBodyNode(index)->setInertia(newInertia);
 }
 
+//==============================================================================
+void Skeleton::setLinkDiagIs(Eigen::VectorXs diag_Is)
+{
+  std::size_t cursor = 0;
+  for(std::size_t i = 0; i < getNumBodyNodes(); i++)
+  {
+    const Inertia& inertia = getBodyNode(i)->getInertia();
+    s_t mass = getBodyNode(i)->getMass();
+    s_t I_XX = diag_Is(cursor++) * mass;
+    s_t I_YY = diag_Is(cursor++) * mass;
+    s_t I_ZZ = diag_Is(cursor++) * mass;
+    Inertia newInertia(
+        inertia.getParameter(dynamics::Inertia::Param::MASS),
+        inertia.getParameter(dynamics::Inertia::Param::COM_X),
+        inertia.getParameter(dynamics::Inertia::Param::COM_Y),
+        inertia.getParameter(dynamics::Inertia::Param::COM_Z),
+        I_XX,
+        I_YY,
+        I_ZZ,
+        inertia.getParameter(dynamics::Inertia::Param::I_XY),
+        inertia.getParameter(dynamics::Inertia::Param::I_XZ),
+        inertia.getParameter(dynamics::Inertia::Param::I_YZ));
+    getBodyNode(i)->setInertia(newInertia);
+  }
+}
+
+void Skeleton::setLinkDiagIIndex(Eigen::Vector3s diag_I, size_t index)
+{
+  const Inertia& inertia = getBodyNode(index)->getInertia();
+  s_t mass = getBodyNode(index)->getMass();
+  s_t I_XX = diag_I(0) * mass;
+  s_t I_YY = diag_I(1) * mass;
+  s_t I_ZZ = diag_I(2) * mass;
+  Inertia newInertia(
+      inertia.getParameter(dynamics::Inertia::Param::MASS),
+      inertia.getParameter(dynamics::Inertia::Param::COM_X),
+      inertia.getParameter(dynamics::Inertia::Param::COM_Y),
+      inertia.getParameter(dynamics::Inertia::Param::COM_Z),
+      I_XX,
+      I_YY,
+      I_ZZ,
+      inertia.getParameter(dynamics::Inertia::Param::I_XY),
+      inertia.getParameter(dynamics::Inertia::Param::I_XZ),
+      inertia.getParameter(dynamics::Inertia::Param::I_YZ));
+  getBodyNode(index)->setInertia(newInertia);
+}
+
 //==============================================================================
 void Skeleton::setLinkMOIs(Eigen::VectorXs mois)
 {
@@ -7227,6 +7357,16 @@ Eigen::VectorXs Skeleton::getDampingCoeffVector()
   return damp_coeffs;
 }
 
+void Skeleton::setDampingCoeffVector(Eigen::VectorXs damp_coeffs)
+{
+  std::vector<dynamics::DegreeOfFreedom*> dofs = getDofs();
+  size_t nDofs = getNumDofs();
+  for(int i = 0; i < nDofs; i++)
+  {
+    dofs[i]->setDampingCoefficient(damp_coeffs(i));
+  }
+}
+
 Eigen::VectorXs Skeleton::getDampingForce()
 {
   Eigen::VectorXs velocities = getVelocities();
@@ -7248,6 +7388,16 @@ Eigen::VectorXs Skeleton::getSpringStiffVector()
   return spring_stiffs;
 }
 
+void Skeleton::setSpringStiffVector(Eigen::VectorXs spring_stiffs)
+{
+  std::vector<dynamics::DegreeOfFreedom*> dofs = getDofs();
+  size_t nDofs = getNumDofs();
+  for(int i = 0; i < nDofs; i++)
+  {
+    dofs[i]->setSpringStiffness(spring_stiffs(i));
+  }
+}
+
 Eigen::VectorXs Skeleton::getRestPositions()
 {
   std::vector<dynamics::DegreeOfFreedom*> dofs = getDofs();

dart/dynamics/Skeleton.hpp

@@ -826,6 +826,8 @@ class Skeleton : public virtual common::VersionCounter,
   /// Returns the size of the getLinkCOMs() vector
   std::size_t getLinkCOMDims();
 
+  std::size_t getLinkDiagIDims();
+
   /// Returns the size of the getLinkMOIs() vector
   std::size_t getLinkMOIDims();
 
@@ -849,17 +851,42 @@ class Skeleton : public virtual common::VersionCounter,
   // This gets particular center-of-mass vectors of a body node
   Eigen::Vector3s getLinkCOMIndex(size_t index);
 
+  Eigen::VectorXs getLinkDiagIs();
+
+  Eigen::Vector3s getLinkDiagIIndex(size_t index);
+
   // This gets all the inertia moment-of-inertia paremeters for all the links in
   // this skeleton concatenated together
   Eigen::VectorXs getLinkMOIs();
 
   // 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 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
   void setControlForceUpperLimits(Eigen::VectorXs limits);
 
@@ -886,16 +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);
 
+  // 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
@@ -1859,13 +1896,19 @@ class Skeleton : public virtual common::VersionCounter,
 
   // Get damping coefficients
   Eigen::VectorXs getDampingCoeffVector();
+
+  // Set Damping Coeff Vector
+  void setDampingCoeffVector(Eigen::VectorXs damp_coeffs);
 
   // Get damping force of the skeleton.
   Eigen::VectorXs getDampingForce();
 
   //Get spring coefficients
   Eigen::VectorXs getSpringStiffVector();
 
+  //set spring coefficinets
+  void setSpringStiffVector(Eigen::VectorXs spring_stiffs);
+
   //Get rest positions
   Eigen::VectorXs getRestPositions();