diff --git a/config/TaskSpaceParams.cfg b/config/TaskSpaceParams.cfg index f5b8012..68f4e14 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", 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) l.add("l_1", double_t, 0, "Friction observer propotional gain for joint 1", 75, 20, 200) 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/JointSpaceCompliantController.cpp b/src/JointSpaceCompliantController.cpp index 7790f89..60d6d30 100644 --- a/src/JointSpaceCompliantController.cpp +++ b/src/JointSpaceCompliantController.cpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include "pinocchio/algorithm/joint-configuration.hpp" @@ -273,7 +274,23 @@ void JointSpaceCompliantController::update(const ros::Time& time, const ros::Dur mDesiredTheta = mDesiredPosition + mJointStiffnessMatrix.inverse() * mGravity; mDesiredThetaDot = mDesiredVelocity; - mTaskEffort = -mJointKMatrix * (mNominalThetaPrev - mDesiredTheta) - mJointDMatrix * (mNominalThetaDotPrev - mDesiredThetaDot); + 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) + } + 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 e0b08bd..4f96dd9 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() << 12, 12, 12, 9, 9, 9; } 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() << 12, 12, 12, 12, 9, 9, 9; } mTaskKMatrix.diagonal() << 200, 200, 200, 100, 100, 100; mTaskDMatrix.diagonal() << 40, 40, 40, 20, 20, 20; @@ -194,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 { @@ -201,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; @@ -361,6 +368,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;