diff --git a/src/JointSpaceCompliantController.cpp b/src/JointSpaceCompliantController.cpp index 78f22ff..60d6d30 100644 --- a/src/JointSpaceCompliantController.cpp +++ b/src/JointSpaceCompliantController.cpp @@ -3,11 +3,11 @@ #include #include #include -#include #include #include #include +#include #include #include "pinocchio/algorithm/joint-configuration.hpp" @@ -275,7 +275,8 @@ void JointSpaceCompliantController::update(const ros::Time& time, const ros::Dur mDesiredThetaDot = mDesiredVelocity; Eigen::VectorXd mErrorTheta = mNominalThetaPrev - mDesiredTheta; - Eigen::VectorXd add_pis = (mErrorTheta.array() > M_PI).select(Eigen::VectorXd::Constant(mNumControlledDofs, -2 * M_PI), Eigen::VectorXd::Constant(mNumControlledDofs, 0)); // account for jump pi -> -pi + Eigen::VectorXd add_pis + = (mErrorTheta.array() > M_PI).select(Eigen::VectorXd::Constant(mNumControlledDofs, -2 * M_PI), Eigen::VectorXd::Constant(mNumControlledDofs, 0)); // account for jump pi -> -pi add_pis += (mErrorTheta.array() <= -M_PI).select(Eigen::VectorXd::Constant(mNumControlledDofs, 2 * M_PI), Eigen::VectorXd::Constant(mNumControlledDofs, 0)); // account for jump -pi -> pi if (mNumControlledDofs == 6) {