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

Update the interface to use the new motion generation class

Browse Source
This commit is contained in:
Juancho
2023-06-27 15:09:17 +09:00
parent c04c20542b
commit fac3db693e
3 changed files with 74 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
include/custom_motion_generation.h
View File

@@ -83,14 +83,21 @@ private:
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);
};

57
src/custom_motion_generation.cpp
View File

@@ -59,6 +59,34 @@ CustomMotionGeneration::CustomMotionGeneration(const double &speed_factor,
q_dot_dot_max_ *= speed_factor_;
}
/**
* @brief CustomMotionGeneration::CustomMotionGeneration
* @param speed_factor
* @param q_dot_initial
* @param q_dot_goal
*/
CustomMotionGeneration::CustomMotionGeneration(const double &speed_factor, const VectorXd &q_dot_initial, const VectorXd &q_dot_goal)
{
n_links_ = _get_dimensions(q_dot_initial, q_dot_goal, q_dot_goal);
solver_ = std::make_unique<DQ_QPOASESSolver>();
constraints_manager_ = std::make_unique<ConstraintsManager>(n_links_);
q_dot_ = q_dot_initial;
H_ = MatrixXd::Identity(n_links_, n_links_);
lb_ = VectorXd::Zero(n_links_);
ub_ = VectorXd::Zero(n_links_);
I_ = MatrixXd::Identity(n_links_, n_links_);
_check_speed_factor(speed_factor);
speed_factor_ = speed_factor;
q_dot_max_ *= speed_factor_;
q_dot_dot_max_ *= speed_factor_;
}
/**
* @brief CustomMotionGeneration::set_proportional_gain
* @param gain
@@ -100,6 +128,7 @@ int CustomMotionGeneration::_get_dimensions(const Eigen::VectorXd &q1,
}
/**
* @brief CustomMotionGeneration::_check_speed_factor
* @param speed_factor
@@ -179,3 +208,31 @@ VectorXd CustomMotionGeneration::compute_new_configuration(const VectorXd &q_goa
return q_;
}
/**
* @brief CustomMotionGeneration::compute_new_configuration_velocities
* @param q_dot_goal
* @param T
* @return
*/
VectorXd CustomMotionGeneration::compute_new_configuration_velocities(const VectorXd &q_dot_goal, const double &T)
{
f_ = 2*gain_*(q_dot_-q_dot_goal);
for (int i=0; i<n_links_;i++)
{
lb_[i] = std::max( -q_dot_dot_max_[i], (1/T)*(-q_dot_max_[i] - q_dot_[i]));
ub_[i] = std::min( q_dot_dot_max_[i], (1/T)*( q_dot_max_[i] - q_dot_[i]));
}
constraints_manager_->add_inequality_constraint(-I_, -lb_);
constraints_manager_->add_inequality_constraint( I_, ub_);
std::tie(A_, b_) = constraints_manager_->get_inequality_constraints();
auto u = solver_->solve_quadratic_program(H_, f_, A_, b_, Aeq_, beq_);
q_dot_ = q_dot_ + T*u;
return q_dot_;
}

11
src/robot_interface_franka.cpp
View File

@@ -473,6 +473,8 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
VectorXd q_dot_initial = VectorXd::Zero(7);
VectorXd q_dot = VectorXd::Zero(7);
desired_joint_velocities_ = VectorXd::Zero(7);
/*
trajectory_generator_sptr_ =
std::make_unique<QuadraticProgramMotionGenerator>(0.8, q_dot_initial, q_dot);
@@ -481,6 +483,12 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
VectorXd K2 = (VectorXd(7)<<n2, n2, n2, n2, n2, n2, n2).finished();
VectorXd K1 = (VectorXd(7)<<n1, n1, n1, n1, 2*n1, 2*n1, 2*n1).finished();
trajectory_generator_sptr_->set_diagonal_gains(K1, K2);
*/
custom_generator_sptr_ = std::make_unique<CustomMotionGeneration>(0.8, q_dot_initial, q_dot);
custom_generator_sptr_->set_proportional_gain(15.0);
_update_status_message("Starting joint velocity control mode EXPERIMENTAL",verbose_);
try {
double time = 0.0;
@@ -490,7 +498,8 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
time += period.toSec();
double T = period.toSec();
auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(desired_joint_velocities_, T);
//auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(desired_joint_velocities_, T);
auto new_u = custom_generator_sptr_->compute_new_configuration_velocities(desired_joint_velocities_, T);
franka::JointVelocities velocities = {{new_u[0], new_u[1],
new_u[2], new_u[3],