Skip to content

fix(franka,controllers): fix osc vibration and trajectory jumping #228

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

Closed
juelg wants to merge 1 commit into from
Closed

fix(franka,controllers): fix osc vibration and trajectory jumping #228

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
77 changes: 50 additions & 27 deletions extensions/rcs_fr3/src/hw/Franka.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,13 +147,14 @@ void PInverse(const Eigen::MatrixXd &M, Eigen::MatrixXd &M_inv,
M_inv = Eigen::MatrixXd(svd.matrixV() * S_inv * svd.matrixU().transpose());
}

void TorqueSafetyGuardFn(std::array<double, 7> &tau_d_array, double min_torque,
double max_torque) {
void TorqueSafetyGuardFn(std::array<double, 7> &tau_d_array,
const std::array<double, 7> &min_torques,
const std::array<double, 7> &max_torques) {
for (size_t i = 0; i < tau_d_array.size(); i++) {
if (tau_d_array[i] < min_torque) {
tau_d_array[i] = min_torque;
} else if (tau_d_array[i] > max_torque) {
tau_d_array[i] = max_torque;
if (tau_d_array[i] < min_torques[i]) {
tau_d_array[i] = min_torques[i];
} else if (tau_d_array[i] > max_torques[i]) {
tau_d_array[i] = max_torques[i];
}
}
}
Expand Down Expand Up @@ -419,18 +420,20 @@ void Franka::osc() {
dist2joint_max = joint_max_.matrix() - q;
dist2joint_min = q - joint_min_.matrix();

double activation_distance = 0.25;
for (int i = 0; i < 7; i++) {
if (dist2joint_max[i] < 0.25 && dist2joint_max[i] > 0.1)
avoidance_force[i] += -avoidance_weights_[i] * dist2joint_max[i];
if (dist2joint_min[i] < 0.25 && dist2joint_min[i] > 0.1)
avoidance_force[i] += avoidance_weights_[i] * dist2joint_min[i];
if (dist2joint_max[i] < activation_distance) {
avoidance_force[i] +=
-avoidance_weights_[i] *
(1 / std::max(dist2joint_max[i], 0.001) - 1 / activation_distance);
}
if (dist2joint_min[i] < activation_distance) {
avoidance_force[i] +=
avoidance_weights_[i] *
(1 / std::max(dist2joint_min[i], 0.001) - 1 / activation_distance);
}
}
tau_d << tau_d + Nullspace * avoidance_force;
for (int i = 0; i < 7; i++) {
if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.;
if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.;
}

std::array<double, 7> tau_d_array{};
Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;

Expand All @@ -439,14 +442,16 @@ void Franka::osc() {
std::chrono::high_resolution_clock::now();
auto time = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);

// clamp the torques to a safe range.
std::array<double, 7> min_torques = {-10.0, -10.0, -10.0, -10.0,
-5.0, -5.0, -5.0};
std::array<double, 7> max_torques = {10.0, 10.0, 10.0, 10.0, 5.0, 5.0, 5.0};
TorqueSafetyGuardFn(tau_d_array, min_torques, max_torques);

// limit the rate of change of the clamped torques.
std::array<double, 7> tau_d_rate_limited = franka::limitRate(
franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);

// deoxys/config/control_config.yml
double min_torque = -5;
double max_torque = 5;
TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque);

return tau_d_rate_limited;
});
}
Expand Down Expand Up @@ -514,16 +519,31 @@ void Franka::joint_controller() {

tau_d << Kp.cwiseProduct(joint_pos_error) - Kd.cwiseProduct(dq);

// Add joint avoidance potential
Eigen::Array<double, 7, 1> avoidance_weights;
avoidance_weights << 1., 1., 1., 1., 1., 10., 10.;
Eigen::Matrix<double, 7, 1> avoidance_force;
avoidance_force.setZero();
Eigen::Matrix<double, 7, 1> dist2joint_max;
Eigen::Matrix<double, 7, 1> dist2joint_min;

dist2joint_max = joint_max_.matrix() - q;
dist2joint_min = q - joint_min_.matrix();

double activation_distance = 0.25;
for (int i = 0; i < 7; i++) {
if (dist2joint_max[i] < 0.1 && tau_d[i] > 0.) tau_d[i] = 0.;
if (dist2joint_min[i] < 0.1 && tau_d[i] < 0.) tau_d[i] = 0.;
if (dist2joint_max[i] < activation_distance) {
avoidance_force[i] +=
-avoidance_weights[i] *
(1 / std::max(dist2joint_max[i], 0.001) - 1 / activation_distance);
}
if (dist2joint_min[i] < activation_distance) {
avoidance_force[i] +=
avoidance_weights[i] *
(1 / std::max(dist2joint_min[i], 0.001) - 1 / activation_distance);
}
}
tau_d << tau_d + avoidance_force;

std::array<double, 7> tau_d_array{};
Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d;
Expand All @@ -533,14 +553,17 @@ void Franka::joint_controller() {
std::chrono::high_resolution_clock::now();
auto time = std::chrono::duration_cast<std::chrono::microseconds>(t2 - t1);

// deoxys/config/control_config.yml
// First clamp the torques to a safe range.
std::array<double, 7> min_torques = {-10.0, -10.0, -10.0, -10.0,
-5.0, -5.0, -5.0};
std::array<double, 7> max_torques = {10.0, 10.0, 10.0, 10.0, 5.0, 5.0, 5.0};
TorqueSafetyGuardFn(tau_d_array, min_torques, max_torques);

// Then limit the rate of change of the clamped torques.
std::array<double, 7> tau_d_rate_limited = franka::limitRate(
franka::kMaxTorqueRate, tau_d_array, robot_state.tau_J_d);

// deoxys/config/control_config.yml
double min_torque = -5;
double max_torque = 5;
TorqueSafetyGuardFn(tau_d_rate_limited, min_torque, max_torque);

return tau_d_rate_limited;
});
}
Expand Down
39 changes: 5 additions & 34 deletions include/rcs/LinearPoseTrajInterpolator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,28 +10,24 @@ class LinearPoseTrajInterpolator {
Eigen::Vector3d p_start_;
Eigen::Vector3d p_goal_;
Eigen::Vector3d last_p_t_;
Eigen::Vector3d prev_p_goal_;

Eigen::Quaterniond q_start_;
Eigen::Quaterniond q_goal_;
Eigen::Quaterniond last_q_t_;
Eigen::Quaterniond prev_q_goal_;

double dt_;
double last_time_;
double max_time_;
double start_time_;
bool start_;
bool first_goal_;

public:
inline LinearPoseTrajInterpolator()
: dt_(0.),
last_time_(0.),
max_time_(1.),
start_time_(0.),
start_(false),
first_goal_(true){};
start_(false){};

inline ~LinearPoseTrajInterpolator(){};

Expand All @@ -49,22 +45,8 @@ class LinearPoseTrajInterpolator {

start_ = false;

if (first_goal_) {
p_start_ = p_start;
q_start_ = q_start;

prev_p_goal_ = p_start;
prev_q_goal_ = q_start;
first_goal_ = false;
} else {
// If the goal is already set, use prev goal as the starting point of
// interpolation.
prev_p_goal_ = p_goal_;
prev_q_goal_ = q_goal_;

p_start_ = prev_p_goal_;
q_start_ = prev_q_goal_;
}
p_start_ = p_start;
q_start_ = q_start;

p_goal_ = p_goal;
q_goal_ = q_goal;
Expand Down Expand Up @@ -104,14 +86,12 @@ class LinearJointPositionTrajInterpolator {
Vector7d q_goal_;

Vector7d last_q_t_;
Vector7d prev_q_goal_;

double dt_;
double last_time_;
double max_time_;
double start_time_;
bool start_;
bool first_goal_;

double interpolation_fraction_; // fraction of actual interpolation within an
// interval
Expand All @@ -122,8 +102,7 @@ class LinearJointPositionTrajInterpolator {
last_time_(0.),
max_time_(1.),
start_time_(0.),
start_(false),
first_goal_(true){};
start_(false){};

inline ~LinearJointPositionTrajInterpolator(){};

Expand All @@ -141,15 +120,7 @@ class LinearJointPositionTrajInterpolator {

start_ = false;

if (first_goal_) {
q_start_ = q_start;
prev_q_goal_ = q_start;
first_goal_ = false;
// std::cout << "First goal of the interpolation" << std::endl;
} else {
prev_q_goal_ = q_goal_;
q_start_ = prev_q_goal_;
}
q_start_ = q_start;
q_goal_ = q_goal;
};

Expand Down