Skip to content

added Null-Space Damping & removed Pi/-Pi Jumps for unlimited rotation joints #1

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 9 commits into
base: main
Choose a base branch
from
Open

added Null-Space Damping & removed Pi/-Pi Jumps for unlimited rotation joints #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
e8ade71
added null-space joint damping
Jan 31, 2024
058d5f8
added Null-Space Joint Damping to Dynamic Reconfigure
Jan 31, 2024
23cb8cf
make joints 1, 3, 5, 7 unlimited rotatiobel in joint impedance control
Feb 2, 2024
25c114e
made C++11 & Eigen compatible
Feb 2, 2024
5dfbae8
increased default Null-Space Damping Params
sophielueth Feb 5, 2024
94ce73c
Merge branch 'main' of https://github.com/sophielueth/gen3_compliant_…
sophielueth Feb 5, 2024
9fb7e00
added 6dof version compatibility for -pi/pi switch
Feb 5, 2024
1b14a5c
clang format adjustment
Feb 7, 2024
983eba7
clang-format adjustements 2
sophielueth Feb 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions config/TaskSpaceParams.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
5 changes: 3 additions & 2 deletions include/gen3_compliant_controllers/TaskSpaceCompliantController.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
19 changes: 18 additions & 1 deletion src/JointSpaceCompliantController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include <gen3_compliant_controllers/JointSpaceCompliantController.hpp>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <math.h>
#include <pluginlib/class_list_macros.h>

#include "pinocchio/algorithm/joint-configuration.hpp"
Expand Down Expand Up @@ -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;
Expand Down
9 changes: 9 additions & 0 deletions src/TaskSpaceCompliantController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -141,19 +141,24 @@ 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
{
mJointStiffnessMatrix.diagonal() << 4000, 4000, 4000, 4000, 3500, 3500, 3500;
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;
Expand Down Expand Up @@ -194,13 +199,15 @@ 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
{
mJointStiffnessMatrix.diagonal() << config.j_0, config.j_1, config.j_2, config.j_3, config.j_4, config.j_5, config.j_6;
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;
Expand Down Expand Up @@ -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;
Expand Down