qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added source files

Browse Source
This commit is contained in:
Juancho
2023-06-07 10:40:50 +09:00
parent 97b2b64d4e
commit 3dc79ae5e6
17 changed files with 2080 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

142
src/motion_generator.cpp Normal file
View File

@@ -0,0 +1,142 @@
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include "MotionGenerator.h"
#include <algorithm>
#include <array>
#include <cmath>
#include <franka/exception.h>
#include <franka/robot.h>
void setDefaultBehavior(franka::Robot& robot) {
robot.setCollisionBehavior(
{{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
{{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
{{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
{{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
}
MotionGenerator::MotionGenerator(const double &speed_factor, const Eigen::VectorXd &q_goal)
{
_check_vector_size(q_goal);
q_goal_ = q_goal;
dq_max_ *= speed_factor;
ddq_max_start_ *= speed_factor;
ddq_max_goal_ *= speed_factor;
dq_max_sync_.setZero();
q_start_.setZero();
delta_q_.setZero();
t_1_sync_.setZero();
t_2_sync_.setZero();
t_f_sync_.setZero();
q_1_.setZero();
}
bool MotionGenerator::calculateDesiredValues(double t, Vector7d* delta_q_d) const {
Vector7i sign_delta_q;
sign_delta_q << delta_q_.cwiseSign().cast<int>();
Vector7d t_d = t_2_sync_ - t_1_sync_;
Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_;
std::array<bool, 7> joint_motion_finished{};
for (size_t i = 0; i < 7; i++) {
if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) {
(*delta_q_d)[i] = 0;
joint_motion_finished[i] = true;
} else {
if (t < t_1_sync_[i]) {
(*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] *
(0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0);
} else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) {
(*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i];
} else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) {
(*delta_q_d)[i] =
delta_q_[i] + 0.5 *
(1.0 / std::pow(delta_t_2_sync[i], 3.0) *
(t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) *
std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) +
(2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) *
dq_max_sync_[i] * sign_delta_q[i];
} else {
(*delta_q_d)[i] = delta_q_[i];
joint_motion_finished[i] = true;
}
}
}
return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(),
[](bool x) { return x; });
}
void MotionGenerator::calculateSynchronizedValues() {
Vector7d dq_max_reach(dq_max_);
Vector7d t_f = Vector7d::Zero();
Vector7d delta_t_2 = Vector7d::Zero();
Vector7d t_1 = Vector7d::Zero();
Vector7d delta_t_2_sync = Vector7d::Zero();
Vector7i sign_delta_q;
sign_delta_q << delta_q_.cwiseSign().cast<int>();
for (size_t i = 0; i < 7; i++) {
if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) +
3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) {
dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] *
(ddq_max_start_[i] * ddq_max_goal_[i]) /
(ddq_max_start_[i] + ddq_max_goal_[i]));
}
t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i];
delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i];
t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i];
}
}
double max_t_f = t_f.maxCoeff();
for (size_t i = 0; i < 7; i++) {
if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) {
double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]);
double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i];
double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i];
double delta = b * b - 4.0 * a * c;
if (delta < 0.0) {
delta = 0.0;
}
dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a);
t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i];
delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i];
t_f_sync_[i] =
(t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]);
t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i];
q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]);
}
}
}
franka::JointPositions MotionGenerator::operator()(const franka::RobotState& robot_state,
franka::Duration period) {
time_ += period.toSec();
if (time_ == 0.0) {
q_start_ = Vector7d(robot_state.q_d.data());
delta_q_ = q_goal_ - q_start_;
calculateSynchronizedValues();
}
Vector7d delta_q_d;
bool motion_finished = calculateDesiredValues(time_, &delta_q_d);
std::array<double, 7> joint_positions;
Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d);
franka::JointPositions output(joint_positions);
output.motion_finished = motion_finished;
return output;
}
void MotionGenerator::_check_vector_size(const Eigen::VectorXd &q)
{
if(q.size() != 7)
{
throw std::runtime_error(std::string("Error in MotionGenerator. Input vector must have size 7"));
}
}