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

general fix and optimization

Browse Source
This commit is contained in:
QuentinLin
2024-07-20 14:33:29 +09:00
parent 9d65b88cb3
commit 0842525bbe
25 changed files with 279 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

103
include/generator/custom_motion_generation.h Normal file
View File

@@ -0,0 +1,103 @@
/*
# Copyright (c) 2023 Juan Jose Quiroz Omana
#
# This file is part of sas_robot_driver_franka.
#
# sas_robot_driver_franka is free software: you can redistribute it and/or modify
# it under the terms of the GNU Lesser General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# sas_robot_driver_franka is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU Lesser General Public License for more details.
#
# You should have received a copy of the GNU Lesser General Public License
# along with sas_robot_driver. If not, see <https://www.gnu.org/licenses/>.
#
# ################################################################
#
# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
#
# ################################################################
#
# Contributors:
# 1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
# - Original implementation
#
# ################################################################
*/
#pragma once
#include <array>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
#include <memory>
#include "constraints_manager.h"
using namespace DQ_robotics;
class CustomMotionGeneration
{
private:
using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
std::unique_ptr<DQ_QPOASESSolver> solver_;
std::unique_ptr<ConstraintsManager> constraints_manager_;
Vector7d q_dot_max_ = (Vector7d() << 2.0, 1.0, 1.5, 1.25, 3, 1.5, 3).finished();
Vector7d q_dot_dot_max_ = (Vector7d() << 8, 8, 8, 8, 8, 8, 8).finished();
const Vector7d q_max_ = ((Vector7d() << 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895).finished());
const Vector7d q_min_ = ((Vector7d() << -2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895).finished());
int n_links_ = 7;
const Vector7d ref_q_dot_max_ = (Vector7d() << 2.0, 1.0, 1.5, 1.25, 3, 1.5, 3).finished();
const Vector7d ref_q_dot_dot_max_ = (Vector7d() << 8, 8, 8, 8, 8, 8, 8).finished();
double speed_factor_;
double gain_ = 1.0;
VectorXd q_;
VectorXd q_dot_;
VectorXd q_dot_prior_ = VectorXd::Zero(7);
VectorXd q_prior_;
MatrixXd H_;
VectorXd lb_;
VectorXd ub_;
MatrixXd A_;
VectorXd b_;
VectorXd f_;
MatrixXd Aeq_;
VectorXd beq_;
MatrixXd I_;
void _check_sizes(const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, const Eigen::VectorXd &q3) const;
int _get_dimensions(const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, const Eigen::VectorXd &q3) const;
void _check_speed_factor(const double& speed_factor) const;
public:
CustomMotionGeneration(const double &speed_factor,
const Eigen::VectorXd &q_initial,
const Eigen::VectorXd &q_dot_initial,
const Eigen::VectorXd &q_goal);
CustomMotionGeneration(const double &speed_factor,
const Eigen::VectorXd &q_dot_initial,
const Eigen::VectorXd &q_dot_goal);
void set_proportional_gain(const double& gain);
VectorXd compute_new_configuration(const VectorXd& q_goal, const double& T);
VectorXd compute_new_configuration_velocities(const VectorXd& q_dot_goal, const double& T);
};

77
include/generator/motion_generator.h Normal file
View File

@@ -0,0 +1,77 @@
// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once
#include <array>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <franka/control_types.h>
#include <franka/duration.h>
#include <franka/robot.h>
#include <franka/robot_state.h>
/**
* @file examples_common.h
* Contains common types and functions for the examples.
*/
/**
* Sets a default collision behavior, joint impedance and Cartesian impedance.
*
* @param[in] robot Robot instance to set behavior on.
*/
void setDefaultBehavior(franka::Robot& robot);
/**
* An example showing how to generate a joint pose motion to a goal position. Adapted from:
* Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots
* (Kogan Page Science Paper edition).
*/
class MotionGenerator {
public:
/**
* Creates a new MotionGenerator instance for a target q.
*
* @param[in] speed_factor General speed factor in range [0, 1].
* @param[in] q_goal Target joint positions.
*/
MotionGenerator(const double &speed_factor, const Eigen::VectorXd &q_goal);
/**
* Sends joint position calculations
*
* @param[in] robot_state Current state of the robot.
* @param[in] period Duration of execution.
*
* @return Joint positions for use inside a control loop.
*/
franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period);
private:
using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
using Vector7i = Eigen::Matrix<int, 7, 1, Eigen::ColMajor>;
void _check_vector_size(const Eigen::VectorXd &q);
bool calculateDesiredValues(double t, Vector7d* delta_q_d) const;
void calculateSynchronizedValues();
static constexpr double kDeltaQMotionFinished = 1e-3; //1e-6;
Vector7d q_goal_;
Vector7d q_start_;
Vector7d delta_q_;
Vector7d dq_max_sync_;
Vector7d t_1_sync_;
Vector7d t_2_sync_;
Vector7d t_f_sync_;
Vector7d q_1_;
double time_ = 0.0;
Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished();
Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished();
};

74
include/generator/quadratic_program_motion_generator.h Normal file
View File

@@ -0,0 +1,74 @@
#pragma once
#include <array>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <iostream>
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
#include <memory>
#include "constraints_manager.h"
using namespace DQ_robotics;
class QuadraticProgramMotionGenerator
{
private:
using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
double speed_factor_;
VectorXd q_;
VectorXd q_dot_;
int n_links_ = 7;
Vector7d q_dot_max_ = (Vector7d() << 2.0, 1.0, 1.5, 1.25, 3, 1.5, 3).finished();
Vector7d q_dot_dot_max_ = (Vector7d() << 8, 8, 8, 8, 8, 8, 8).finished();
double n2_ = 1.0;
double n1_ = 2000*std::sqrt(n2_);
VectorXd N1_ = (VectorXd(7)<<n1_, n1_, n1_, n1_, n1_, n1_, n1_).finished();
VectorXd N2_ = (VectorXd(7)<<n2_, n2_, n2_, n2_, n2_, n2_, n2_).finished();
MatrixXd K1_;
MatrixXd K2_;
MatrixXd H_;
VectorXd lb_;
VectorXd ub_;
MatrixXd A_;
VectorXd b_;
VectorXd f_;
MatrixXd Aeq_;
VectorXd beq_;
MatrixXd I_;
void _check_sizes(const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, const Eigen::VectorXd &q3) const;
int _get_dimensions(const Eigen::VectorXd &q1, const Eigen::VectorXd &q2, const Eigen::VectorXd &q3) const;
void _check_gains();
std::unique_ptr<DQ_QPOASESSolver> solver_;
std::unique_ptr<ConstraintsManager> constraints_manager_;
void _check_speed_factor(const double& speed_factor) const;
public:
QuadraticProgramMotionGenerator(const double &speed_factor,
const Eigen::VectorXd &q_initial,
const Eigen::VectorXd &q_dot_initial,
const Eigen::VectorXd &q_goal);
QuadraticProgramMotionGenerator(const double &speed_factor,
const Eigen::VectorXd &q_dot_initial,
const Eigen::VectorXd &q_dot_goal);
VectorXd compute_new_configuration(const VectorXd& q_goal, const double& T);
VectorXd compute_new_configuration_velocities(const VectorXd& q_dot_goal, const double& T);
void set_gains(const double& n1, const double& n2);
void set_diagonal_gains(const VectorXd& K1, const VectorXd& K2);
};