From e8ade717cd305916c7711c219ba9df1343c8ddb1 Mon Sep 17 00:00:00 2001 From: sclueth Date: Tue, 30 Jan 2024 18:21:51 -0800 Subject: [PATCH 1/8] added null-space joint damping --- .../TaskSpaceCompliantController.hpp | 5 +++-- src/TaskSpaceCompliantController.cpp | 7 +++++++ 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/include/gen3_compliant_controllers/TaskSpaceCompliantController.hpp b/include/gen3_compliant_controllers/TaskSpaceCompliantController.hpp index 7b2798b..ecccbff 100644 --- a/include/gen3_compliant_controllers/TaskSpaceCompliantController.hpp +++ b/include/gen3_compliant_controllers/TaskSpaceCompliantController.hpp @@ -76,8 +76,9 @@ class TaskSpaceCompliantController : public controller_interface::MultiInterface Eigen::MatrixXd mRotorInertiaMatrix; // Rotor inertia matrix Eigen::MatrixXd mFrictionL; // Friction observer matrix 1 Eigen::MatrixXd mFrictionLp; // Friction observer matrix 2 - Eigen::MatrixXd mTaskKMatrix; // Task compliance proportional gain matrix - Eigen::MatrixXd mTaskDMatrix; // Task compliance derivative gain matrix + Eigen::MatrixXd mTaskKMatrix; // Task compliance proportional gain matrix (virtual cartesian stiffness) + Eigen::MatrixXd mTaskDMatrix; // Task compliance derivative gain matrix (virtual cartesian damping) + Eigen::MatrixXd mJointDMatrix; // Task Null-Space joint damping matrix long long int mCount; // Used during initialization diff --git a/src/TaskSpaceCompliantController.cpp b/src/TaskSpaceCompliantController.cpp index e0b08bd..5e58022 100644 --- a/src/TaskSpaceCompliantController.cpp +++ b/src/TaskSpaceCompliantController.cpp @@ -141,12 +141,16 @@ bool TaskSpaceCompliantController::init(hardware_interface::RobotHW* robot, ros: mTaskDMatrix.resize(6, 6); mTaskDMatrix.setZero(); + mJointDMatrix.resize(mNumControlledDofs, mNumControlledDofs); + mJointDMatrix.setZero(); + if (mNumControlledDofs == 6) { mJointStiffnessMatrix.diagonal() << 4000, 4000, 4000, 3500, 3500, 3500; mRotorInertiaMatrix.diagonal() << 0.3, 0.3, 0.3, 0.18, 0.18, 0.2; mFrictionL.diagonal() << 75, 75, 75, 40, 40, 40; mFrictionLp.diagonal() << 5, 5, 5, 4, 4, 4; + mJointDMatrix.diagonal() << 4, 4, 4, 3, 3, 3; } else { @@ -154,6 +158,7 @@ bool TaskSpaceCompliantController::init(hardware_interface::RobotHW* robot, ros: mRotorInertiaMatrix.diagonal() << 0.3, 0.3, 0.3, 0.3, 0.18, 0.18, 0.2; mFrictionL.diagonal() << 75, 75, 75, 75, 40, 40, 40; mFrictionLp.diagonal() << 5, 5, 5, 5, 4, 4, 4; + mJointDMatrix.diagonal() << 4, 4, 4, 4, 3, 3, 3; } mTaskKMatrix.diagonal() << 200, 200, 200, 100, 100, 100; mTaskDMatrix.diagonal() << 40, 40, 40, 20, 20, 20; @@ -361,6 +366,8 @@ void TaskSpaceCompliantController::update(const ros::Time& time, const ros::Dura } mTaskEffort = dart_nominal_jacobian.transpose() * (-mTaskKMatrix * dart_error - mTaskDMatrix * (dart_nominal_jacobian * mNominalThetaDotPrev)); + Eigen::MatrixXd dart_nominal_jacobian_pseudo_inverse = dart_nominal_jacobian.completeOrthogonalDecomposition().pseudoInverse(); + mTaskEffort = mTaskEffort - (Eigen::MatrixXd::Identity(mNumControlledDofs, mNumControlledDofs) - dart_nominal_jacobian_pseudo_inverse*dart_nominal_jacobian) * mJointDMatrix * mNominalThetaDotPrev; double step_time; step_time = 0.001; From 058d5f858d4dd7c44493e94da7e457a7bf7c5385 Mon Sep 17 00:00:00 2001 From: sclueth Date: Tue, 30 Jan 2024 18:23:38 -0800 Subject: [PATCH 2/8] added Null-Space Joint Damping to Dynamic Reconfigure --- config/TaskSpaceParams.cfg | 9 +++++++++ src/TaskSpaceCompliantController.cpp | 2 ++ 2 files changed, 11 insertions(+) diff --git a/config/TaskSpaceParams.cfg b/config/TaskSpaceParams.cfg index f5b8012..68d890e 100644 --- a/config/TaskSpaceParams.cfg +++ b/config/TaskSpaceParams.cfg @@ -40,6 +40,15 @@ kd.add("d_roll", double_t, 0, "Damping Gain for Rotation about X", 20, 1, 6 kd.add("d_pitch", double_t, 0, "Damping Gain for Rotation about Y", 20, 1, 60) kd.add("d_yaw", double_t, 0, "Damping Gain for Rotation about Z", 20, 1, 60) +kqd = gen.add_group("Kqd") # Null-Space joint damping gain +kqd.add("qd_0", double_t, 0, "Joint 0 Null-Space Joint Damping", 4, 2, 10) +kqd.add("qd_1", double_t, 0, "Joint 1 Null-Space Joint Damping", 4, 2, 10) +kqd.add("qd_2", double_t, 0, "Joint 2 Null-Space Joint Damping", 4, 2, 10) +kqd.add("qd_3", double_t, 0, "Joint 3 Null-Space Joint Damping", 4, 2, 10) +kqd.add("qd_4", double_t, 0, "Joint 4 Null-Space Joint Damping", 3, 1.5, 10) +kqd.add("qd_5", double_t, 0, "Joint 5 Null-Space Joint Damping", 3, 1.5, 10) +kqd.add("qd_6", double_t, 0, "Joint 6 Null-Space Joint Damping", 3, 1.5, 10) + l = gen.add_group("L") # friction observer proportional gain l.add("l_0", double_t, 0, "Friction observer propotional gain for joint 0", 75, 20, 200) l.add("l_1", double_t, 0, "Friction observer propotional gain for joint 1", 75, 20, 200) diff --git a/src/TaskSpaceCompliantController.cpp b/src/TaskSpaceCompliantController.cpp index 5e58022..1c1c36a 100644 --- a/src/TaskSpaceCompliantController.cpp +++ b/src/TaskSpaceCompliantController.cpp @@ -199,6 +199,7 @@ void TaskSpaceCompliantController::dynamicReconfigureCallback(gen3_compliant_con mRotorInertiaMatrix.diagonal() << config.b_0, config.b_1, config.b_2, config.b_3, config.b_4, config.b_5; mFrictionL.diagonal() << config.l_0, config.l_1, config.l_2, config.l_3, config.l_4, config.l_5; mFrictionLp.diagonal() << config.lp_0, config.lp_1, config.lp_2, config.lp_3, config.lp_4, config.lp_5; + mJointDMatrix.diagonal() << config.qd_0, config.qd_1, config.qd_2, config.qd_3, config.qd_4, config.qd_5; } else { @@ -206,6 +207,7 @@ void TaskSpaceCompliantController::dynamicReconfigureCallback(gen3_compliant_con mRotorInertiaMatrix.diagonal() << config.b_0, config.b_1, config.b_2, config.b_3, config.b_4, config.b_5, config.b_6; mFrictionL.diagonal() << config.l_0, config.l_1, config.l_2, config.l_3, config.l_4, config.l_5, config.l_6; mFrictionLp.diagonal() << config.lp_0, config.lp_1, config.lp_2, config.lp_3, config.lp_4, config.lp_5, config.lp_6; + mJointDMatrix.diagonal() << config.qd_0, config.qd_1, config.qd_2, config.qd_3, config.qd_4, config.qd_5, config.qd_6; } mTaskKMatrix.diagonal() << config.k_x, config.k_y, config.k_z, config.k_roll, config.k_pitch, config.k_yaw; mTaskDMatrix.diagonal() << config.d_x, config.d_y, config.d_z, config.d_roll, config.d_pitch, config.d_yaw; From 23cb8cfb0237f57ca12251b9fb331c32bca3da68 Mon Sep 17 00:00:00 2001 From: sclueth Date: Thu, 1 Feb 2024 18:14:11 -0800 Subject: [PATCH 3/8] make joints 1, 3, 5, 7 unlimited rotatiobel in joint impedance control --- src/JointSpaceCompliantController.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/JointSpaceCompliantController.cpp b/src/JointSpaceCompliantController.cpp index 7790f89..5f7bc53 100644 --- a/src/JointSpaceCompliantController.cpp +++ b/src/JointSpaceCompliantController.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -273,7 +274,12 @@ void JointSpaceCompliantController::update(const ros::Time& time, const ros::Dur mDesiredTheta = mDesiredPosition + mJointStiffnessMatrix.inverse() * mGravity; mDesiredThetaDot = mDesiredVelocity; - mTaskEffort = -mJointKMatrix * (mNominalThetaPrev - mDesiredTheta) - mJointDMatrix * (mNominalThetaDotPrev - mDesiredThetaDot); + std::vector ind{0, 2, 4, 6}; // make joints 1, 3, 5, 7 (indices 0, 2, 4, 6) limitless rotationable + Eigen::VectorXd mErrorTheta = mNominalThetaPrev - mDesiredTheta; + Eigen::VectorXd two_pi = 2*std::numbers::pi*Eigen::VectorXd::Constant(mNumcontrolledDof, 1); + mErrorTheta(ind) = (mErrorTheta(ind).array() > std::numbers::pi).select(mErrorTheta(ind) - two_pi, mErrorTheta(ind); // account for jump pi -> -pi + mErrorTheta(ind) = (mErrorTheta(ind).array() <= -std::numbers::pi).select(mErrorTheta(ind) + two_pi, mErrorTheta(ind); // account for jump -pi -> pi + mTaskEffort = -mJointKMatrix * mErrorTheta - mJointDMatrix * (mNominalThetaDotPrev - mDesiredThetaDot); double step_time; step_time = 0.001; From 25c114ede46d89a5e9d9941f5b0f75734a3ee911 Mon Sep 17 00:00:00 2001 From: sclueth Date: Thu, 1 Feb 2024 21:04:58 -0800 Subject: [PATCH 4/8] made C++11 & Eigen compatible --- src/JointSpaceCompliantController.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/JointSpaceCompliantController.cpp b/src/JointSpaceCompliantController.cpp index 5f7bc53..8e47b12 100644 --- a/src/JointSpaceCompliantController.cpp +++ b/src/JointSpaceCompliantController.cpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include @@ -274,12 +274,11 @@ void JointSpaceCompliantController::update(const ros::Time& time, const ros::Dur mDesiredTheta = mDesiredPosition + mJointStiffnessMatrix.inverse() * mGravity; mDesiredThetaDot = mDesiredVelocity; - std::vector ind{0, 2, 4, 6}; // make joints 1, 3, 5, 7 (indices 0, 2, 4, 6) limitless rotationable Eigen::VectorXd mErrorTheta = mNominalThetaPrev - mDesiredTheta; - Eigen::VectorXd two_pi = 2*std::numbers::pi*Eigen::VectorXd::Constant(mNumcontrolledDof, 1); - mErrorTheta(ind) = (mErrorTheta(ind).array() > std::numbers::pi).select(mErrorTheta(ind) - two_pi, mErrorTheta(ind); // account for jump pi -> -pi - mErrorTheta(ind) = (mErrorTheta(ind).array() <= -std::numbers::pi).select(mErrorTheta(ind) + two_pi, mErrorTheta(ind); // account for jump -pi -> pi - mTaskEffort = -mJointKMatrix * mErrorTheta - mJointDMatrix * (mNominalThetaDotPrev - mDesiredThetaDot); + 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 + add_pis[1] = 0; add_pis[3] = 0; add_pis[5] = 0; // respect joint limits at pi/-pi for joints 2, 4, 6 (indices 1, 3, 5) + mTaskEffort = -mJointKMatrix * (mErrorTheta + add_pis)- mJointDMatrix * (mNominalThetaDotPrev - mDesiredThetaDot); double step_time; step_time = 0.001; From 5dfbae8215d8f51503c2a9848ed75100f2384e27 Mon Sep 17 00:00:00 2001 From: sophielueth Date: Mon, 5 Feb 2024 11:40:03 -0800 Subject: [PATCH 5/8] increased default Null-Space Damping Params --- config/TaskSpaceParams.cfg | 14 +++++++------- src/TaskSpaceCompliantController.cpp | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/config/TaskSpaceParams.cfg b/config/TaskSpaceParams.cfg index 68d890e..68f4e14 100644 --- a/config/TaskSpaceParams.cfg +++ b/config/TaskSpaceParams.cfg @@ -41,13 +41,13 @@ kd.add("d_pitch", double_t, 0, "Damping Gain for Rotation about Y", 20, 1, 6 kd.add("d_yaw", double_t, 0, "Damping Gain for Rotation about Z", 20, 1, 60) kqd = gen.add_group("Kqd") # Null-Space joint damping gain -kqd.add("qd_0", double_t, 0, "Joint 0 Null-Space Joint Damping", 4, 2, 10) -kqd.add("qd_1", double_t, 0, "Joint 1 Null-Space Joint Damping", 4, 2, 10) -kqd.add("qd_2", double_t, 0, "Joint 2 Null-Space Joint Damping", 4, 2, 10) -kqd.add("qd_3", double_t, 0, "Joint 3 Null-Space Joint Damping", 4, 2, 10) -kqd.add("qd_4", double_t, 0, "Joint 4 Null-Space Joint Damping", 3, 1.5, 10) -kqd.add("qd_5", double_t, 0, "Joint 5 Null-Space Joint Damping", 3, 1.5, 10) -kqd.add("qd_6", double_t, 0, "Joint 6 Null-Space Joint Damping", 3, 1.5, 10) +kqd.add("qd_0", double_t, 0, "Joint 0 Null-Space Joint Damping", 12, 2, 10) +kqd.add("qd_1", double_t, 0, "Joint 1 Null-Space Joint Damping", 12, 2, 10) +kqd.add("qd_2", double_t, 0, "Joint 2 Null-Space Joint Damping", 12, 2, 10) +kqd.add("qd_3", double_t, 0, "Joint 3 Null-Space Joint Damping", 12, 2, 10) +kqd.add("qd_4", double_t, 0, "Joint 4 Null-Space Joint Damping", 9, 1.5, 10) +kqd.add("qd_5", double_t, 0, "Joint 5 Null-Space Joint Damping", 9, 1.5, 10) +kqd.add("qd_6", double_t, 0, "Joint 6 Null-Space Joint Damping", 9, 1.5, 10) l = gen.add_group("L") # friction observer proportional gain l.add("l_0", double_t, 0, "Friction observer propotional gain for joint 0", 75, 20, 200) diff --git a/src/TaskSpaceCompliantController.cpp b/src/TaskSpaceCompliantController.cpp index 1c1c36a..4d4880e 100644 --- a/src/TaskSpaceCompliantController.cpp +++ b/src/TaskSpaceCompliantController.cpp @@ -150,7 +150,7 @@ bool TaskSpaceCompliantController::init(hardware_interface::RobotHW* robot, ros: mRotorInertiaMatrix.diagonal() << 0.3, 0.3, 0.3, 0.18, 0.18, 0.2; mFrictionL.diagonal() << 75, 75, 75, 40, 40, 40; mFrictionLp.diagonal() << 5, 5, 5, 4, 4, 4; - mJointDMatrix.diagonal() << 4, 4, 4, 3, 3, 3; + mJointDMatrix.diagonal() << 12, 12, 12, 9, 9, 9; } else { @@ -158,7 +158,7 @@ bool TaskSpaceCompliantController::init(hardware_interface::RobotHW* robot, ros: mRotorInertiaMatrix.diagonal() << 0.3, 0.3, 0.3, 0.3, 0.18, 0.18, 0.2; mFrictionL.diagonal() << 75, 75, 75, 75, 40, 40, 40; mFrictionLp.diagonal() << 5, 5, 5, 5, 4, 4, 4; - mJointDMatrix.diagonal() << 4, 4, 4, 4, 3, 3, 3; + mJointDMatrix.diagonal() << 12, 12, 12, 12, 9, 9, 9; } mTaskKMatrix.diagonal() << 200, 200, 200, 100, 100, 100; mTaskDMatrix.diagonal() << 40, 40, 40, 20, 20, 20; From 9fb7e005297b8f915ecc5101d32e3116c4179076 Mon Sep 17 00:00:00 2001 From: sclueth Date: Mon, 5 Feb 2024 15:42:26 -0800 Subject: [PATCH 6/8] added 6dof version compatibility for -pi/pi switch --- src/JointSpaceCompliantController.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/JointSpaceCompliantController.cpp b/src/JointSpaceCompliantController.cpp index 8e47b12..a03c264 100644 --- a/src/JointSpaceCompliantController.cpp +++ b/src/JointSpaceCompliantController.cpp @@ -277,7 +277,11 @@ void JointSpaceCompliantController::update(const ros::Time& time, const ros::Dur 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 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[1] = 0; add_pis[3] = 0; add_pis[5] = 0; // respect joint limits at pi/-pi for joints 2, 4, 6 (indices 1, 3, 5) + if (mNumControlledDofs == 6) { + add_pis[1] = 0; add_pis[2] = 0; add_pis[4] = 0; // respect joint limits at pi/-pi for joints 2, 3, 5 (indices 1, 2, 4) + } else { + add_pis[1] = 0; add_pis[3] = 0; add_pis[5] = 0; // respect joint limits at pi/-pi for joints 2, 4, 6 (indices 1, 3, 5) + } mTaskEffort = -mJointKMatrix * (mErrorTheta + add_pis)- mJointDMatrix * (mNominalThetaDotPrev - mDesiredThetaDot); double step_time; From 1b14a5cbfa937ea4e4633025e557217bf2f16179 Mon Sep 17 00:00:00 2001 From: sclueth Date: Wed, 7 Feb 2024 11:22:33 -0800 Subject: [PATCH 7/8] clang format adjustment --- src/JointSpaceCompliantController.cpp | 21 ++++++++++++++------- src/TaskSpaceCompliantController.cpp | 2 +- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/src/JointSpaceCompliantController.cpp b/src/JointSpaceCompliantController.cpp index a03c264..78f22ff 100644 --- a/src/JointSpaceCompliantController.cpp +++ b/src/JointSpaceCompliantController.cpp @@ -275,14 +275,21 @@ 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 - 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) { - add_pis[1] = 0; add_pis[2] = 0; add_pis[4] = 0; // respect joint limits at pi/-pi for joints 2, 3, 5 (indices 1, 2, 4) - } else { - add_pis[1] = 0; add_pis[3] = 0; add_pis[5] = 0; // respect joint limits at pi/-pi for joints 2, 4, 6 (indices 1, 3, 5) + 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) + { + add_pis[1] = 0; + add_pis[2] = 0; + add_pis[4] = 0; // respect joint limits at pi/-pi for joints 2, 3, 5 (indices 1, 2, 4) + } + else + { + add_pis[1] = 0; + add_pis[3] = 0; + add_pis[5] = 0; // respect joint limits at pi/-pi for joints 2, 4, 6 (indices 1, 3, 5) } - mTaskEffort = -mJointKMatrix * (mErrorTheta + add_pis)- mJointDMatrix * (mNominalThetaDotPrev - mDesiredThetaDot); + mTaskEffort = -mJointKMatrix * (mErrorTheta + add_pis) - mJointDMatrix * (mNominalThetaDotPrev - mDesiredThetaDot); double step_time; step_time = 0.001; diff --git a/src/TaskSpaceCompliantController.cpp b/src/TaskSpaceCompliantController.cpp index 4d4880e..4f96dd9 100644 --- a/src/TaskSpaceCompliantController.cpp +++ b/src/TaskSpaceCompliantController.cpp @@ -369,7 +369,7 @@ void TaskSpaceCompliantController::update(const ros::Time& time, const ros::Dura mTaskEffort = dart_nominal_jacobian.transpose() * (-mTaskKMatrix * dart_error - mTaskDMatrix * (dart_nominal_jacobian * mNominalThetaDotPrev)); Eigen::MatrixXd dart_nominal_jacobian_pseudo_inverse = dart_nominal_jacobian.completeOrthogonalDecomposition().pseudoInverse(); - mTaskEffort = mTaskEffort - (Eigen::MatrixXd::Identity(mNumControlledDofs, mNumControlledDofs) - dart_nominal_jacobian_pseudo_inverse*dart_nominal_jacobian) * mJointDMatrix * mNominalThetaDotPrev; + mTaskEffort = mTaskEffort - (Eigen::MatrixXd::Identity(mNumControlledDofs, mNumControlledDofs) - dart_nominal_jacobian_pseudo_inverse * dart_nominal_jacobian) * mJointDMatrix * mNominalThetaDotPrev; double step_time; step_time = 0.001; From 983eba7e9ba1bb7ba7631c52906f3db3c3ca3dda Mon Sep 17 00:00:00 2001 From: sophielueth Date: Wed, 7 Feb 2024 14:41:35 -0800 Subject: [PATCH 8/8] clang-format adjustements 2 --- src/JointSpaceCompliantController.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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) {