refactor impulses

dart/constraint/BallJointConstraint.cpp

@@ -302,7 +302,7 @@ void BallJointConstraint::unexcite()
 }
 
 //==============================================================================
-void BallJointConstraint::applyImpulse(s_t* _lambda)
+void BallJointConstraint::applyImpulse(Eigen::VectorXs _lambda)
 {
   mOldX[0] = _lambda[0];
   mOldX[1] = _lambda[1];

dart/constraint/BallJointConstraint.hpp

@@ -35,8 +35,8 @@
 
 #include <Eigen/Dense>
 
-#include "dart/math/MathTypes.hpp"
 #include "dart/constraint/JointConstraint.hpp"
+#include "dart/math/MathTypes.hpp"
 
 namespace dart {
 namespace constraint {
@@ -49,17 +49,18 @@ class BallJointConstraint : public JointConstraint
   /// Constructor that takes one body and the joint position in the world frame
   /// \param[in] _body
   /// \param[in] _jointPos Joint position expressed in world frame
-  BallJointConstraint(dynamics::BodyNode* _body,
-                      const Eigen::Vector3s& _jointPos);
-
+  BallJointConstraint(
+      dynamics::BodyNode* _body, const Eigen::Vector3s& _jointPos);
 
   /// Constructor that takes two bodies and the joint position in the frame of
   /// _body1
   /// \param[in] _body1
   /// \param[in] _body2
   /// \param[in] _jointPos Joint position expressed in world frame
-  BallJointConstraint(dynamics::BodyNode* _body1, dynamics::BodyNode* _body2,
-                      const Eigen::Vector3s& _jointPos);
+  BallJointConstraint(
+      dynamics::BodyNode* _body1,
+      dynamics::BodyNode* _body2,
+      const Eigen::Vector3s& _jointPos);
 
   /// Destructor
   virtual ~BallJointConstraint();
@@ -88,7 +89,7 @@ class BallJointConstraint : public JointConstraint
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* _lambda) override;
+  void applyImpulse(Eigen::VectorXs _lambda) override;
 
   // Documentation inherited
   bool isActive() const override;
@@ -128,8 +129,7 @@ class BallJointConstraint : public JointConstraint
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 };
 
-}  // namespace constraint
-}  // namespace dart
-
-#endif  // DART_CONSTRAINT_BALLJOINTCONSTRAINT_HPP_
+} // namespace constraint
+} // namespace dart
 
+#endif // DART_CONSTRAINT_BALLJOINTCONSTRAINT_HPP_

dart/constraint/BoxedLcpConstraintSolver.cpp

@@ -349,7 +349,7 @@ LcpInputs BoxedLcpConstraintSolver::buildLcpInputs(ConstrainedGroup& group)
 }
 
 //==============================================================================
-std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
+Eigen::MatrixXs BoxedLcpConstraintSolver::solveLcp(
     LcpInputs lcpInputs, ConstrainedGroup& group)
 {
   const std::size_t numConstraints = group.getNumConstraints();
@@ -736,9 +736,7 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
   }
 
   // Initialize the vector of constraint impulses we will eventually return.
-  // Each ith element of the vector will contain a pointer to the constraint
-  // impulse to be applied for the ith constraint.
-  std::vector<s_t*> constraintImpulses;
+  Eigen::MatrixXs constraintImpulses = Eigen::MatrixXs::Zero(numConstraints, 3);
 
   // Collect the final solved constraint impulses to apply per constraint.
   // TODO(mguo): Make impulse magnitudes all have 3 elements regardless of
@@ -748,6 +746,8 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
     const ConstraintBasePtr& constraint = group.getConstraint(i);
     if (constraint->isContactConstraint())
     {
+      s_t* lambda = mX.data() + mOffset[i];
+      constraintImpulses(i, 0) = lambda[0];
       std::shared_ptr<ContactConstraint> contactConstraint
           = std::static_pointer_cast<ContactConstraint>(constraint);
       // getContact() returns a const, which is generally what we want, but in
@@ -766,12 +766,14 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
           = contactConstraint->isFrictionOn();
       if (contactConstraint->isFrictionOn())
       {
+        constraintImpulses(i, 1) = lambda[1];
+        constraintImpulses(i, 2) = lambda[2];
         const_cast<collision::Contact*>(&contactConstraint->getContact())
             ->lcpResultTangent1
-            = mX(mOffset[i] + 1);
+            = lambda[1];
         const_cast<collision::Contact*>(&contactConstraint->getContact())
             ->lcpResultTangent2
-            = mX(mOffset[i] + 2);
+            = lambda[2];
         const ContactConstraint::TangentBasisMatrix D
             = contactConstraint->getTangentBasisMatrixODE(
                 contactConstraint->getContact().normal);
@@ -783,13 +785,12 @@ std::vector<s_t*> BoxedLcpConstraintSolver::solveLcp(
             = D.col(1);
       }
     }
-    constraintImpulses.push_back(mX.data() + mOffset[i]);
   }
   return constraintImpulses;
 }
 
 //==============================================================================
-std::vector<s_t*> BoxedLcpConstraintSolver::solveConstrainedGroup(
+Eigen::MatrixXs BoxedLcpConstraintSolver::solveConstrainedGroup(
     ConstrainedGroup& group)
 {
   LcpInputs lcpInputs = buildLcpInputs(group);

dart/constraint/BoxedLcpConstraintSolver.hpp

@@ -114,13 +114,13 @@ class BoxedLcpConstraintSolver : public ConstraintSolver
   virtual void setCachedLCPSolution(Eigen::VectorXs X) override;
 
   // Documentation inherited.
-  std::vector<s_t*> solveConstrainedGroup(ConstrainedGroup& group) override;
+  Eigen::MatrixXs solveConstrainedGroup(ConstrainedGroup& group) override;
 
   /// Build the inputs to the LCP from the constraint group.
   LcpInputs buildLcpInputs(ConstrainedGroup& group);
 
   /// Setup and solve an LCP to enforce the constraints on the ConstrainedGroup.
-  std::vector<s_t*> solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group);
+  Eigen::MatrixXs solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group);
 
 protected:
   /// Boxed LCP solver

dart/constraint/ConstraintBase.hpp

@@ -98,7 +98,7 @@ class ConstraintBase
   virtual void unexcite() = 0;
 
   /// Apply computed constraint impulse to constrained skeletons
-  virtual void applyImpulse(s_t* lambda) = 0;
+  virtual void applyImpulse(Eigen::VectorXs lambda) = 0;
 
   /// Return true if this constraint is active
   virtual bool isActive() const = 0;

dart/constraint/ConstraintSolver.cpp

@@ -804,20 +804,20 @@ void ConstraintSolver::solveConstrainedGroups()
     if (0u == n)
       continue;
 
-    std::vector<s_t*> impulses = solveConstrainedGroup(constraintGroup);
+    Eigen::MatrixXs impulses = solveConstrainedGroup(constraintGroup);
     applyConstraintImpulses(constraintGroup.getConstraints(), impulses);
   }
 }
 
 //==============================================================================
 void ConstraintSolver::applyConstraintImpulses(
-    std::vector<ConstraintBasePtr> constraints, std::vector<s_t*> impulses)
+    std::vector<ConstraintBasePtr> constraints, Eigen::MatrixXs impulses)
 {
   const std::size_t numConstraints = constraints.size();
   for (std::size_t i = 0; i < numConstraints; ++i)
   {
     const ConstraintBasePtr& constraint = constraints[i];
-    constraint->applyImpulse(impulses[i]);
+    constraint->applyImpulse(impulses.row(i));
     constraint->excite();
   }
 }

dart/constraint/ConstraintSolver.hpp

@@ -206,11 +206,11 @@ class ConstraintSolver
   void solveConstrainedGroups();
 
   // Solve for constraint impulses to apply to each constraint in group.
-  virtual std::vector<s_t*> solveConstrainedGroup(ConstrainedGroup& group) = 0;
+  virtual Eigen::MatrixXs solveConstrainedGroup(ConstrainedGroup& group) = 0;
 
   /// Apply constraint impulses to each constraint.
   void applyConstraintImpulses(
-      std::vector<ConstraintBasePtr> constraints, std::vector<s_t*> impulses);
+      std::vector<ConstraintBasePtr> constraints, Eigen::MatrixXs impulses);
 
   /// Get constrained groups.
   const std::vector<ConstrainedGroup>& getConstrainedGroups() const;

dart/constraint/ContactConstraint.cpp

@@ -627,7 +627,7 @@ void ContactConstraint::unexcite()
 }
 
 //==============================================================================
-void ContactConstraint::applyImpulse(s_t* lambda)
+void ContactConstraint::applyImpulse(Eigen::VectorXs lambda)
 {
   //----------------------------------------------------------------------------
   // Friction case

dart/constraint/ContactConstraint.hpp

@@ -142,7 +142,7 @@ class ContactConstraint : public ConstraintBase
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* lambda) override;
+  void applyImpulse(Eigen::VectorXs lambda) override;
 
   // Documentation inherited
   dynamics::SkeletonPtr getRootSkeleton() const override;

dart/constraint/DantzigLCPSolver.cpp

@@ -218,8 +218,8 @@ void DantzigLCPSolver::solve(ConstrainedGroup* _group)
   for (std::size_t i = 0; i < numConstraints; ++i)
   {
     const ConstraintBasePtr& constraint = _group->getConstraint(i);
-    constraint->applyImpulse(x + offset[i]);
-    constraint->excite();
+    // constraint->applyImpulse(x + offset[i]);
+    // constraint->excite();
   }
 
   delete[] offset;

dart/constraint/JointCoulombFrictionConstraint.cpp

@@ -39,7 +39,7 @@
 #include "dart/dynamics/Joint.hpp"
 #include "dart/dynamics/Skeleton.hpp"
 
-#define DART_CFM     1e-9
+#define DART_CFM 1e-9
 
 namespace dart {
 namespace constraint {
@@ -84,13 +84,15 @@ void JointCoulombFrictionConstraint::setConstraintForceMixing(s_t _cfm)
   if (_cfm < 1e-9)
   {
     dtwarn << "Constraint force mixing parameter[" << _cfm
-           << "] is lower than 1e-9. " << "It is set to 1e-9." << std::endl;
+           << "] is lower than 1e-9. "
+           << "It is set to 1e-9." << std::endl;
     mConstraintForceMixing = 1e-9;
   }
   if (_cfm > 1.0)
   {
     dtwarn << "Constraint force mixing parameter[" << _cfm
-           << "] is greater than 1.0. " << "It is set to 1.0." << std::endl;
+           << "] is greater than 1.0. "
+           << "It is set to 1.0." << std::endl;
     mConstraintForceMixing = 1.0;
   }
 
@@ -126,7 +128,7 @@ void JointCoulombFrictionConstraint::update()
       // redundancy.
 
       // Note: Coulomb friction is force not impulse
-      mUpperBound[i] =  mJoint->getCoulombFriction(i) * timeStep;
+      mUpperBound[i] = mJoint->getCoulombFriction(i) * timeStep;
       mLowerBound[i] = -mUpperBound[i];
 
       if (mActive[i])
@@ -205,14 +207,14 @@ void JointCoulombFrictionConstraint::applyUnitImpulse(std::size_t _index)
 }
 
 //==============================================================================
-void JointCoulombFrictionConstraint::getVelocityChange(s_t* _delVel,
-                                                       bool _withCfm)
+void JointCoulombFrictionConstraint::getVelocityChange(
+    s_t* _delVel, bool _withCfm)
 {
   assert(_delVel != nullptr && "Null pointer is not allowed.");
 
   std::size_t localIndex = 0;
   std::size_t dof = mJoint->getNumDofs();
-  for (std::size_t i = 0; i < dof ; ++i)
+  for (std::size_t i = 0; i < dof; ++i)
   {
     if (mActive[i] == false)
       continue;
@@ -229,8 +231,8 @@ void JointCoulombFrictionConstraint::getVelocityChange(s_t* _delVel,
   // varaible in ODE
   if (_withCfm)
   {
-    _delVel[mAppliedImpulseIndex] += _delVel[mAppliedImpulseIndex]
-                                     * mConstraintForceMixing;
+    _delVel[mAppliedImpulseIndex]
+        += _delVel[mAppliedImpulseIndex] * mConstraintForceMixing;
   }
 
   assert(localIndex == mDim);
@@ -249,17 +251,17 @@ void JointCoulombFrictionConstraint::unexcite()
 }
 
 //==============================================================================
-void JointCoulombFrictionConstraint::applyImpulse(s_t* _lambda)
+void JointCoulombFrictionConstraint::applyImpulse(Eigen::VectorXs _lambda)
 {
   std::size_t localIndex = 0;
   std::size_t dof = mJoint->getNumDofs();
-  for (std::size_t i = 0; i < dof ; ++i)
+  for (std::size_t i = 0; i < dof; ++i)
   {
     if (mActive[i] == false)
       continue;
 
     mJoint->setConstraintImpulse(
-          i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]);
+        i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]);
 
     mOldX[i] = _lambda[localIndex];
 

dart/constraint/JointCoulombFrictionConstraint.hpp

@@ -40,7 +40,7 @@ namespace dart {
 namespace dynamics {
 class BodyNode;
 class Joint;
-}  // namespace dynamics
+} // namespace dynamics
 
 namespace constraint {
 
@@ -95,7 +95,7 @@ class JointCoulombFrictionConstraint : public ConstraintBase
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* _lambda) override;
+  void applyImpulse(Eigen::VectorXs _lambda) override;
 
   // Documentation inherited
   dynamics::SkeletonPtr getRootSkeleton() const override;
@@ -137,8 +137,7 @@ class JointCoulombFrictionConstraint : public ConstraintBase
   static s_t mConstraintForceMixing;
 };
 
-}  // namespace constraint
-}  // namespace dart
-
-#endif  // DART_CONSTRAINT_JOINTCOULOMBFRICTIONCONSTRAINT_HPP_
+} // namespace constraint
+} // namespace dart
 
+#endif // DART_CONSTRAINT_JOINTCOULOMBFRICTIONCONSTRAINT_HPP_

dart/constraint/JointLimitConstraint.cpp

@@ -360,7 +360,7 @@ void JointLimitConstraint::unexcite()
 }
 
 //==============================================================================
-void JointLimitConstraint::applyImpulse(s_t* _lambda)
+void JointLimitConstraint::applyImpulse(Eigen::VectorXs _lambda)
 {
   std::size_t localIndex = 0;
   std::size_t dof = mJoint->getNumDofs();

dart/constraint/JointLimitConstraint.hpp

@@ -40,7 +40,7 @@ namespace dart {
 namespace dynamics {
 class BodyNode;
 class Joint;
-}  // namespace dynamics
+} // namespace dynamics
 
 namespace constraint {
 
@@ -114,7 +114,7 @@ class JointLimitConstraint : public ConstraintBase
   void unexcite() override;
 
   // Documentation inherited
-  void applyImpulse(s_t* _lambda) override;
+  void applyImpulse(Eigen::VectorXs _lambda) override;
 
   // Documentation inherited
   dynamics::SkeletonPtr getRootSkeleton() const override;
@@ -169,8 +169,7 @@ class JointLimitConstraint : public ConstraintBase
   static s_t mConstraintForceMixing;
 };
 
-}  // namespace constraint
-}  // namespace dart
-
-#endif  // DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_
+} // namespace constraint
+} // namespace dart
 
+#endif // DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_