From 30f096a1a6e2a864283520031a81a8456bfb9298 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 16:55:37 -0800 Subject: [PATCH 1/5] refactor impulses --- dart/constraint/BallJointConstraint.cpp | 2 +- dart/constraint/BallJointConstraint.hpp | 22 +++---- dart/constraint/BoxedLcpConstraintSolver.cpp | 17 +++--- dart/constraint/BoxedLcpConstraintSolver.hpp | 4 +- dart/constraint/ConstraintBase.hpp | 2 +- dart/constraint/ConstraintSolver.cpp | 6 +- dart/constraint/ConstraintSolver.hpp | 4 +- dart/constraint/ContactConstraint.cpp | 2 +- dart/constraint/ContactConstraint.hpp | 2 +- dart/constraint/DantzigLCPSolver.cpp | 4 +- .../JointCoulombFrictionConstraint.cpp | 26 +++++---- .../JointCoulombFrictionConstraint.hpp | 11 ++-- dart/constraint/JointLimitConstraint.cpp | 2 +- dart/constraint/JointLimitConstraint.hpp | 11 ++-- dart/constraint/MimicMotorConstraint.cpp | 7 +-- dart/constraint/MimicMotorConstraint.hpp | 2 +- dart/constraint/PGSLCPSolver.cpp | 4 +- dart/constraint/ServoMotorConstraint.cpp | 23 ++++---- dart/constraint/ServoMotorConstraint.hpp | 11 ++-- dart/constraint/SoftContactConstraint.cpp | 2 +- dart/constraint/SoftContactConstraint.hpp | 14 ++--- dart/constraint/WeldJointConstraint.cpp | 57 ++++++++----------- dart/constraint/WeldJointConstraint.hpp | 11 ++-- .../constraint/BoxedLcpConstraintSolver.cpp | 2 +- .../constraint/ConstraintBase.cpp | 2 +- .../constraint/ConstraintSolver.cpp | 2 +- 26 files changed, 121 insertions(+), 131 deletions(-) diff --git a/dart/constraint/BallJointConstraint.cpp b/dart/constraint/BallJointConstraint.cpp index 5b607645e..a864e0e44 100644 --- a/dart/constraint/BallJointConstraint.cpp +++ b/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]; diff --git a/dart/constraint/BallJointConstraint.hpp b/dart/constraint/BallJointConstraint.hpp index 09e6a439e..33d752212 100644 --- a/dart/constraint/BallJointConstraint.hpp +++ b/dart/constraint/BallJointConstraint.hpp @@ -35,8 +35,8 @@ #include -#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_ diff --git a/dart/constraint/BoxedLcpConstraintSolver.cpp b/dart/constraint/BoxedLcpConstraintSolver.cpp index 6cd898a44..d9a234dfd 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.cpp +++ b/dart/constraint/BoxedLcpConstraintSolver.cpp @@ -349,7 +349,7 @@ LcpInputs BoxedLcpConstraintSolver::buildLcpInputs(ConstrainedGroup& group) } //============================================================================== -std::vector BoxedLcpConstraintSolver::solveLcp( +Eigen::MatrixXs BoxedLcpConstraintSolver::solveLcp( LcpInputs lcpInputs, ConstrainedGroup& group) { const std::size_t numConstraints = group.getNumConstraints(); @@ -736,9 +736,7 @@ std::vector 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 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 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 = std::static_pointer_cast(constraint); // getContact() returns a const, which is generally what we want, but in @@ -766,12 +766,14 @@ std::vector BoxedLcpConstraintSolver::solveLcp( = contactConstraint->isFrictionOn(); if (contactConstraint->isFrictionOn()) { + constraintImpulses(i, 1) = lambda[1]; + constraintImpulses(i, 2) = lambda[2]; const_cast(&contactConstraint->getContact()) ->lcpResultTangent1 - = mX(mOffset[i] + 1); + = lambda[1]; const_cast(&contactConstraint->getContact()) ->lcpResultTangent2 - = mX(mOffset[i] + 2); + = lambda[2]; const ContactConstraint::TangentBasisMatrix D = contactConstraint->getTangentBasisMatrixODE( contactConstraint->getContact().normal); @@ -783,13 +785,12 @@ std::vector BoxedLcpConstraintSolver::solveLcp( = D.col(1); } } - constraintImpulses.push_back(mX.data() + mOffset[i]); } return constraintImpulses; } //============================================================================== -std::vector BoxedLcpConstraintSolver::solveConstrainedGroup( +Eigen::MatrixXs BoxedLcpConstraintSolver::solveConstrainedGroup( ConstrainedGroup& group) { LcpInputs lcpInputs = buildLcpInputs(group); diff --git a/dart/constraint/BoxedLcpConstraintSolver.hpp b/dart/constraint/BoxedLcpConstraintSolver.hpp index 71d520d88..e515667e2 100644 --- a/dart/constraint/BoxedLcpConstraintSolver.hpp +++ b/dart/constraint/BoxedLcpConstraintSolver.hpp @@ -114,13 +114,13 @@ class BoxedLcpConstraintSolver : public ConstraintSolver virtual void setCachedLCPSolution(Eigen::VectorXs X) override; // Documentation inherited. - std::vector 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 solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group); + Eigen::MatrixXs solveLcp(LcpInputs lcpInputs, ConstrainedGroup& group); protected: /// Boxed LCP solver diff --git a/dart/constraint/ConstraintBase.hpp b/dart/constraint/ConstraintBase.hpp index e4fe35ff2..219e8c602 100644 --- a/dart/constraint/ConstraintBase.hpp +++ b/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; diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 2af247094..cf8e153d8 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -804,20 +804,20 @@ void ConstraintSolver::solveConstrainedGroups() if (0u == n) continue; - std::vector impulses = solveConstrainedGroup(constraintGroup); + Eigen::MatrixXs impulses = solveConstrainedGroup(constraintGroup); applyConstraintImpulses(constraintGroup.getConstraints(), impulses); } } //============================================================================== void ConstraintSolver::applyConstraintImpulses( - std::vector constraints, std::vector impulses) + std::vector 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(); } } diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp index 92b48171f..4f7f0efc1 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/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 solveConstrainedGroup(ConstrainedGroup& group) = 0; + virtual Eigen::MatrixXs solveConstrainedGroup(ConstrainedGroup& group) = 0; /// Apply constraint impulses to each constraint. void applyConstraintImpulses( - std::vector constraints, std::vector impulses); + std::vector constraints, Eigen::MatrixXs impulses); /// Get constrained groups. const std::vector& getConstrainedGroups() const; diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index 8a571473e..1545a2a8d 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/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 diff --git a/dart/constraint/ContactConstraint.hpp b/dart/constraint/ContactConstraint.hpp index 860bd374c..e6ae20a83 100644 --- a/dart/constraint/ContactConstraint.hpp +++ b/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; diff --git a/dart/constraint/DantzigLCPSolver.cpp b/dart/constraint/DantzigLCPSolver.cpp index 99f658d81..ff7ef0f34 100644 --- a/dart/constraint/DantzigLCPSolver.cpp +++ b/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; diff --git a/dart/constraint/JointCoulombFrictionConstraint.cpp b/dart/constraint/JointCoulombFrictionConstraint.cpp index 86842cee6..0ab11e658 100644 --- a/dart/constraint/JointCoulombFrictionConstraint.cpp +++ b/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]; diff --git a/dart/constraint/JointCoulombFrictionConstraint.hpp b/dart/constraint/JointCoulombFrictionConstraint.hpp index 85a2a40c7..067f9250a 100644 --- a/dart/constraint/JointCoulombFrictionConstraint.hpp +++ b/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_ diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp index bc785a12e..59da747f2 100644 --- a/dart/constraint/JointLimitConstraint.cpp +++ b/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(); diff --git a/dart/constraint/JointLimitConstraint.hpp b/dart/constraint/JointLimitConstraint.hpp index 3afac7e42..6e230865e 100644 --- a/dart/constraint/JointLimitConstraint.hpp +++ b/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_ diff --git a/dart/constraint/MimicMotorConstraint.cpp b/dart/constraint/MimicMotorConstraint.cpp index f99793390..16a5ad446 100644 --- a/dart/constraint/MimicMotorConstraint.cpp +++ b/dart/constraint/MimicMotorConstraint.cpp @@ -34,12 +34,11 @@ #include -#include "dart/external/odelcpsolver/lcp.h" - #include "dart/common/Console.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/Joint.hpp" #include "dart/dynamics/Skeleton.hpp" +#include "dart/external/odelcpsolver/lcp.h" #define DART_CFM 1e-9 @@ -129,7 +128,7 @@ void MimicMotorConstraint::update() { s_t timeStep = mJoint->getSkeleton()->getTimeStep(); s_t qError = mMimicJoint->getPosition(i) * mMultiplier + mOffset - - mJoint->getPosition(i); + - mJoint->getPosition(i); s_t desiredVelocity = math::clip( qError / timeStep, mJoint->getVelocityLowerLimit(i), @@ -262,7 +261,7 @@ void MimicMotorConstraint::unexcite() } //============================================================================== -void MimicMotorConstraint::applyImpulse(s_t* lambda) +void MimicMotorConstraint::applyImpulse(Eigen::VectorXs lambda) { std::size_t localIndex = 0; std::size_t dof = mJoint->getNumDofs(); diff --git a/dart/constraint/MimicMotorConstraint.hpp b/dart/constraint/MimicMotorConstraint.hpp index 05fd60782..75bf1ebe2 100644 --- a/dart/constraint/MimicMotorConstraint.hpp +++ b/dart/constraint/MimicMotorConstraint.hpp @@ -99,7 +99,7 @@ class MimicMotorConstraint : 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; diff --git a/dart/constraint/PGSLCPSolver.cpp b/dart/constraint/PGSLCPSolver.cpp index ef93d639c..52c18d1ac 100644 --- a/dart/constraint/PGSLCPSolver.cpp +++ b/dart/constraint/PGSLCPSolver.cpp @@ -185,8 +185,8 @@ void PGSLCPSolver::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; diff --git a/dart/constraint/ServoMotorConstraint.cpp b/dart/constraint/ServoMotorConstraint.cpp index 41ab1bc6d..090bbf356 100644 --- a/dart/constraint/ServoMotorConstraint.cpp +++ b/dart/constraint/ServoMotorConstraint.cpp @@ -34,14 +34,13 @@ #include -#include "dart/external/odelcpsolver/lcp.h" - #include "dart/common/Console.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/Joint.hpp" #include "dart/dynamics/Skeleton.hpp" +#include "dart/external/odelcpsolver/lcp.h" -#define DART_CFM 1e-9 +#define DART_CFM 1e-9 namespace dart { namespace constraint { @@ -86,14 +85,16 @@ void ServoMotorConstraint::setConstraintForceMixing(s_t cfm) { dtwarn << "[ServoMotorConstraint::setConstraintForceMixing] " << "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 << "[ServoMotorConstraint::setConstraintForceMixing] " << "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; } @@ -214,7 +215,7 @@ void ServoMotorConstraint::getVelocityChange(s_t* delVel, bool withCfm) 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; @@ -231,8 +232,8 @@ void ServoMotorConstraint::getVelocityChange(s_t* delVel, bool withCfm) // varaible in ODE if (withCfm) { - delVel[mAppliedImpulseIndex] += delVel[mAppliedImpulseIndex] - * mConstraintForceMixing; + delVel[mAppliedImpulseIndex] + += delVel[mAppliedImpulseIndex] * mConstraintForceMixing; } assert(localIndex == mDim); @@ -251,17 +252,17 @@ void ServoMotorConstraint::unexcite() } //============================================================================== -void ServoMotorConstraint::applyImpulse(s_t* lambda) +void ServoMotorConstraint::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]); // TODO(JS): consider to add Joint::addConstraintImpulse() mOldX[i] = lambda[localIndex]; diff --git a/dart/constraint/ServoMotorConstraint.hpp b/dart/constraint/ServoMotorConstraint.hpp index f5de7450e..1932affca 100644 --- a/dart/constraint/ServoMotorConstraint.hpp +++ b/dart/constraint/ServoMotorConstraint.hpp @@ -40,7 +40,7 @@ namespace dart { namespace dynamics { class BodyNode; class Joint; -} // namespace dynamics +} // namespace dynamics namespace constraint { @@ -95,7 +95,7 @@ class ServoMotorConstraint : 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; @@ -139,8 +139,7 @@ class ServoMotorConstraint : public ConstraintBase static s_t mConstraintForceMixing; }; -} // namespace constraint -} // namespace dart - -#endif // DART_CONSTRAINT_SERVOMOTORCONSTRAINT_HPP_ +} // namespace constraint +} // namespace dart +#endif // DART_CONSTRAINT_SERVOMOTORCONSTRAINT_HPP_ diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index b407c378e..cd00d9f61 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -733,7 +733,7 @@ void SoftContactConstraint::unexcite() } //============================================================================== -void SoftContactConstraint::applyImpulse(s_t* _lambda) +void SoftContactConstraint::applyImpulse(Eigen::VectorXs _lambda) { // TODO(JS): Need to optimize code diff --git a/dart/constraint/SoftContactConstraint.hpp b/dart/constraint/SoftContactConstraint.hpp index 0a9d4f188..f7d5f8dc4 100644 --- a/dart/constraint/SoftContactConstraint.hpp +++ b/dart/constraint/SoftContactConstraint.hpp @@ -33,23 +33,22 @@ #ifndef DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_ #define DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_ +#include "dart/collision/CollisionDetector.hpp" #include "dart/constraint/ConstraintBase.hpp" - #include "dart/math/MathTypes.hpp" -#include "dart/collision/CollisionDetector.hpp" namespace dart { -namespace collision{ +namespace collision { class SoftCollisionInfo; -} // namespace collision +} // namespace collision namespace dynamics { class BodyNode; class SoftBodyNode; class PointMass; class Skeleton; -} // namespace dynamics +} // namespace dynamics namespace constraint { @@ -128,7 +127,7 @@ class SoftContactConstraint : 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; @@ -247,5 +246,4 @@ class SoftContactConstraint : public ConstraintBase } // namespace constraint } // namespace dart -#endif // DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_ - +#endif // DART_CONSTRAINT_SOFTCONTACTCONSTRAINT_HPP_ diff --git a/dart/constraint/WeldJointConstraint.cpp b/dart/constraint/WeldJointConstraint.cpp index f7aa1fe5e..c6365523a 100644 --- a/dart/constraint/WeldJointConstraint.cpp +++ b/dart/constraint/WeldJointConstraint.cpp @@ -32,10 +32,9 @@ #include "dart/constraint/WeldJointConstraint.hpp" -#include "dart/external/odelcpsolver/lcp.h" - #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/Skeleton.hpp" +#include "dart/external/odelcpsolver/lcp.h" namespace dart { namespace constraint { @@ -59,11 +58,11 @@ WeldJointConstraint::WeldJointConstraint(dynamics::BodyNode* _body) } //============================================================================== -WeldJointConstraint::WeldJointConstraint(dynamics::BodyNode* _body1, - dynamics::BodyNode* _body2) +WeldJointConstraint::WeldJointConstraint( + dynamics::BodyNode* _body1, dynamics::BodyNode* _body2) : JointConstraint(_body1, _body2), - mRelativeTransform(_body2->getTransform().inverse() - * _body1->getTransform()), + mRelativeTransform( + _body2->getTransform().inverse() * _body1->getTransform()), mViolation(Eigen::Vector6s::Zero()), mJacobian1(Eigen::Matrix6s::Identity()), mAppliedImpulseIndex(0) @@ -108,18 +107,17 @@ void WeldJointConstraint::update() // Update Jacobian for body2 if (mBodyNode2) { - Eigen::Isometry3s T12 = mBodyNode1->getTransform().inverse() - * mBodyNode2->getTransform(); + Eigen::Isometry3s T12 + = mBodyNode1->getTransform().inverse() * mBodyNode2->getTransform(); mJacobian2 = math::AdTJac(T12, mJacobian1); } // Update position constraint error if (mBodyNode2) { - const Eigen::Isometry3s& violationT - = mRelativeTransform.inverse() - * mBodyNode2->getTransform().inverse() - * mBodyNode1->getTransform(); + const Eigen::Isometry3s& violationT = mRelativeTransform.inverse() + * mBodyNode2->getTransform().inverse() + * mBodyNode1->getTransform(); mViolation = math::logMap(violationT); } @@ -206,13 +204,15 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) if (mBodyNode2->isReactive()) { mBodyNode1->getSkeleton()->updateBiasImpulse( - mBodyNode1, mJacobian1.row(_index), - mBodyNode2, -mJacobian2.row(_index)); + mBodyNode1, + mJacobian1.row(_index), + mBodyNode2, + -mJacobian2.row(_index)); } else { mBodyNode1->getSkeleton()->updateBiasImpulse( - mBodyNode1, mJacobian1.row(_index)); + mBodyNode1, mJacobian1.row(_index)); } } else @@ -220,7 +220,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) if (mBodyNode2->isReactive()) { mBodyNode2->getSkeleton()->updateBiasImpulse( - mBodyNode2, -mJacobian2.row(_index)); + mBodyNode2, -mJacobian2.row(_index)); } else { @@ -237,7 +237,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) { mBodyNode1->getSkeleton()->clearConstraintImpulses(); mBodyNode1->getSkeleton()->updateBiasImpulse( - mBodyNode1, mJacobian1.row(_index)); + mBodyNode1, mJacobian1.row(_index)); mBodyNode1->getSkeleton()->updateVelocityChange(); } @@ -245,7 +245,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) { mBodyNode2->getSkeleton()->clearConstraintImpulses(); mBodyNode2->getSkeleton()->updateBiasImpulse( - mBodyNode2, -mJacobian2.row(_index)); + mBodyNode2, -mJacobian2.row(_index)); mBodyNode2->getSkeleton()->updateVelocityChange(); } } @@ -256,7 +256,7 @@ void WeldJointConstraint::applyUnitImpulse(std::size_t _index) mBodyNode1->getSkeleton()->clearConstraintImpulses(); mBodyNode1->getSkeleton()->updateBiasImpulse( - mBodyNode1, mJacobian1.row(_index)); + mBodyNode1, mJacobian1.row(_index)); mBodyNode1->getSkeleton()->updateVelocityChange(); } @@ -270,14 +270,12 @@ void WeldJointConstraint::getVelocityChange(s_t* _vel, bool _withCfm) assert(isActive()); Eigen::Vector6s velChange = Eigen::Vector6s::Zero(); - if (mBodyNode1->getSkeleton()->isImpulseApplied() - && mBodyNode1->isReactive()) + if (mBodyNode1->getSkeleton()->isImpulseApplied() && mBodyNode1->isReactive()) { velChange += mBodyNode1->getBodyVelocityChange(); } - if (mBodyNode2 - && mBodyNode2->getSkeleton()->isImpulseApplied() + if (mBodyNode2 && mBodyNode2->getSkeleton()->isImpulseApplied() && mBodyNode2->isReactive()) { velChange -= mJacobian2 * mBodyNode2->getBodyVelocityChange(); @@ -290,8 +288,8 @@ void WeldJointConstraint::getVelocityChange(s_t* _vel, bool _withCfm) // varaible in ODE if (_withCfm) { - _vel[mAppliedImpulseIndex] += _vel[mAppliedImpulseIndex] - * mConstraintForceMixing; + _vel[mAppliedImpulseIndex] + += _vel[mAppliedImpulseIndex] * mConstraintForceMixing; } } @@ -322,7 +320,7 @@ void WeldJointConstraint::unexcite() } //============================================================================== -void WeldJointConstraint::applyImpulse(s_t* _lambda) +void WeldJointConstraint::applyImpulse(Eigen::VectorXs _lambda) { mOldX[0] = _lambda[0]; mOldX[1] = _lambda[1]; @@ -332,12 +330,7 @@ void WeldJointConstraint::applyImpulse(s_t* _lambda) mOldX[5] = _lambda[5]; Eigen::Vector6s imp; - imp << _lambda[0], - _lambda[1], - _lambda[2], - _lambda[3], - _lambda[4], - _lambda[5]; + imp << _lambda[0], _lambda[1], _lambda[2], _lambda[3], _lambda[4], _lambda[5]; mBodyNode1->addConstraintImpulse(imp); diff --git a/dart/constraint/WeldJointConstraint.hpp b/dart/constraint/WeldJointConstraint.hpp index 77ea6f178..5cfd7ebff 100644 --- a/dart/constraint/WeldJointConstraint.hpp +++ b/dart/constraint/WeldJointConstraint.hpp @@ -35,8 +35,8 @@ #include -#include "dart/math/MathTypes.hpp" #include "dart/constraint/JointConstraint.hpp" +#include "dart/math/MathTypes.hpp" namespace dart { namespace constraint { @@ -85,7 +85,7 @@ class WeldJointConstraint : 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; @@ -120,8 +120,7 @@ class WeldJointConstraint : public JointConstraint EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace constraint -} // namespace dart - -#endif // DART_CONSTRAINT_WELDJOINTCONSTRAINT_HPP_ +} // namespace constraint +} // namespace dart +#endif // DART_CONSTRAINT_WELDJOINTCONSTRAINT_HPP_ diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index fa62e2f81..5918284c1 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -77,7 +77,7 @@ void BoxedLcpConstraintSolver(py::module& m) "solveLcp", +[](dart::constraint::BoxedLcpConstraintSolver* self, dart::constraint::LcpInputs lcpInputs, - dart::constraint::ConstrainedGroup& group) -> std::vector { + dart::constraint::ConstrainedGroup& group) -> Eigen::MatrixXs { return self->solveLcp(lcpInputs, group); }); } diff --git a/python/_nimblephysics/constraint/ConstraintBase.cpp b/python/_nimblephysics/constraint/ConstraintBase.cpp index 7408ebe12..23d0f2bde 100644 --- a/python/_nimblephysics/constraint/ConstraintBase.cpp +++ b/python/_nimblephysics/constraint/ConstraintBase.cpp @@ -81,7 +81,7 @@ void ConstraintBase(py::module& m) +[](dart::constraint::ConstraintBase* self) { self->unexcite(); }) .def( "applyImpulse", - +[](dart::constraint::ConstraintBase* self, s_t* lambda) { + +[](dart::constraint::ConstraintBase* self, Eigen::VectorXs lambda) { self->applyImpulse(lambda); }, ::py::arg("lambda")) diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index 757c0fa09..e9d404d32 100644 --- a/python/_nimblephysics/constraint/ConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp @@ -201,7 +201,7 @@ void ConstraintSolver(py::module& m) "applyConstraintImpulses", +[](dart::constraint::ConstraintSolver* self, std::vector constraints, - std::vector impulses) { + Eigen::VectorXs impulses) { self->applyConstraintImpulses(constraints, impulses); }) .def( From 1aa1f0123606ab0e5ff68ab306b686040f724004 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 17:08:21 -0800 Subject: [PATCH 2/5] add eigen import --- python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp index 5918284c1..18424efca 100644 --- a/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/BoxedLcpConstraintSolver.cpp @@ -33,6 +33,7 @@ #include #include #include +#include #include #include From f415caf7b8b53749bfd95ff5240e46c0a9d6adb9 Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 17:12:33 -0800 Subject: [PATCH 3/5] one more eigen import --- python/_nimblephysics/constraint/ConstraintSolver.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index e9d404d32..c822092f6 100644 --- a/python/_nimblephysics/constraint/ConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include From 6578761565122ab18525a68fecc0490015f3dc2a Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 18:00:33 -0800 Subject: [PATCH 4/5] matrix instead of vector --- python/_nimblephysics/constraint/ConstraintSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/_nimblephysics/constraint/ConstraintSolver.cpp b/python/_nimblephysics/constraint/ConstraintSolver.cpp index c822092f6..22d7b5133 100644 --- a/python/_nimblephysics/constraint/ConstraintSolver.cpp +++ b/python/_nimblephysics/constraint/ConstraintSolver.cpp @@ -202,7 +202,7 @@ void ConstraintSolver(py::module& m) "applyConstraintImpulses", +[](dart::constraint::ConstraintSolver* self, std::vector constraints, - Eigen::VectorXs impulses) { + Eigen::MatrixXs impulses) { self->applyConstraintImpulses(constraints, impulses); }) .def( From c12130dfaeed9f4763e37086f438a42e4f14b79a Mon Sep 17 00:00:00 2001 From: Michelle Guo Date: Wed, 5 Jan 2022 19:54:41 -0800 Subject: [PATCH 5/5] fix variable type for unittest --- unittests/unit/test_ConstraintSolver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/unittests/unit/test_ConstraintSolver.cpp b/unittests/unit/test_ConstraintSolver.cpp index ab0cd3e52..b17d2c405 100644 --- a/unittests/unit/test_ConstraintSolver.cpp +++ b/unittests/unit/test_ConstraintSolver.cpp @@ -70,7 +70,7 @@ TEST(ConstraintSolver, SIMPLE) // Solve constraint impulses. for (auto constraintGroup : solver->getConstrainedGroups()) { - std::vector impulses = solver->solveConstrainedGroup(constraintGroup); + Eigen::MatrixXs impulses = solver->solveConstrainedGroup(constraintGroup); EXPECT_TRUE(impulses.size() > 0); solver->applyConstraintImpulses(constraintGroup.getConstraints(), impulses); }