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"));
}
}

159
src/quadratic_program_motion_generator.cpp Normal file
View File

@@ -0,0 +1,159 @@
#include "QuadraticProgramMotionGenerator.h"
QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor,
const Eigen::VectorXd &q_initial,
const Eigen::VectorXd &q_dot_initial,
const Eigen::VectorXd &q_goal)
{
n_links_ = _get_dimensions(q_initial, q_dot_initial, q_goal);
solver_ = std::make_unique<DQ_QPOASESSolver>(DQ_QPOASESSolver());
constraints_manager_ = std::make_unique<ConstraintsManager>(ConstraintsManager(n_links_));
_check_gains();
q_ = q_initial;
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_;
K1_ = N1_.asDiagonal();
K2_ = N2_.asDiagonal();
}
QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor, const VectorXd &q_dot_initial, const VectorXd &q_dot_goal)
{
n_links_ = _get_dimensions(q_dot_initial, q_dot_initial, q_dot_goal);
solver_ = std::make_unique<DQ_QPOASESSolver>(DQ_QPOASESSolver());
constraints_manager_ = std::make_unique<ConstraintsManager>(ConstraintsManager(n_links_));
_check_gains();
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_;
K1_ = N1_.asDiagonal();
K2_ = N2_.asDiagonal();
}
void QuadraticProgramMotionGenerator::_check_speed_factor(const double& speed_factor) const
{
if (speed_factor > 1.0)
{
throw std::runtime_error("Speed factor must be <= 1.0");
}
if (speed_factor <= 0.0)
{
throw std::runtime_error("Speed factor must be > 0.0");
}
}
int QuadraticProgramMotionGenerator::_get_dimensions(const Eigen::VectorXd &q1,
const Eigen::VectorXd &q2,
const Eigen::VectorXd &q3) const
{
_check_sizes(q1, q2, q3);
return q1.size();
}
void QuadraticProgramMotionGenerator::set_gains(const double& n1, const double& n2)
{
n1_ = n1;
n2_ = n2;
_check_gains() ;
N1_ <<n1_, n1_, n1_, n1_, n1_, n1_, n1_;
N2_ <<n2_, n2_, n2_, n2_, n2_, n2_, n2_;
K1_ = N1_.asDiagonal();
K2_ = N2_.asDiagonal();
}
void QuadraticProgramMotionGenerator::set_diagonal_gains(const VectorXd& K1, const VectorXd& K2)
{
K1_ = K1.asDiagonal();
K2_ = K2.asDiagonal();
}
void QuadraticProgramMotionGenerator::_check_sizes(const Eigen::VectorXd &q1,
const Eigen::VectorXd &q2,
const Eigen::VectorXd &q3) const
{
if (q1.size() != q2.size() or q1.size() != q3.size() or q2.size() != q3.size())
{
throw std::runtime_error(std::string("Wrong sizes in vectors. "));
}
}
void QuadraticProgramMotionGenerator::_check_gains()
{
if ( std::pow(n1_, 2) - 4*n2_ <= 0)
{
throw std::runtime_error(std::string("Wrong gains!!!. You need to use gains such that n1^2 - 4*n2 > 0. "));
}
}
VectorXd QuadraticProgramMotionGenerator::compute_new_configuration(const VectorXd& q_goal,
const double& T)
{
f_ = K1_*(q_dot_ + K2_*(q_-q_goal)); //f_ = n1_*(q_dot_ + n2_*(q_-q_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_ = q_ +T*q_dot_ +0.5*std::pow(T,2)*u;
q_dot_ = q_dot_ + T*u;
return q_;
}
VectorXd QuadraticProgramMotionGenerator::compute_new_configuration_velocities(const VectorXd &q_dot_goal, const double &T)
{
f_ = 2*K1_*(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_;
}

789
src/robot_interface_franka.cpp Normal file
View File

@@ -0,0 +1,789 @@
#include "robot_interface_franka.h"
/**
* @brief robot_driver_franka::robot_driver_franka
* @param ROBOT_IP The IP address of the FCI
* @param mode The operation mode {None, PositionControl}.
* @param hand The hand option {ONFinished, OFF}.
*/
RobotInterfaceFranka::RobotInterfaceFranka(const std::string &ROBOT_IP,
const MODE& mode,
const HAND& hand):ip_(ROBOT_IP),mode_(mode)
{
_set_driver_mode(mode);
if (hand == RobotInterfaceFranka::HAND::ON)
{
gripper_sptr_ = std::make_shared<franka::Gripper>(ip_);
}
robot_sptr_ = std::make_shared<franka::Robot>(ip_);
_setDefaultRobotBehavior();
/*
std::cout<<"---------------------------------------------------------------"<<std::endl;
std::cout<<"The connection is not automatic. Expected Workflow: "<<std::endl;
std::cout<<"franka_driver.connect(); "<<std::endl;
std::cout<<"franka_driver.initialize(); "<<std::endl;
std::cout<<" "<<std::endl;
std::cout<<"franka_driver.deinitialize(); "<<std::endl;
std::cout<<"franka_driver.disconnect(); "<<std::endl;
std::cout<<"---------------------------------------------------------------"<<std::endl;
*/
std::cout<<"-----------------------------------------------------------------"<<std::endl;
std::cout<<"RobotInterfaceFranka is brought to you by Juan Jose Quiroz Omana."<<std::endl;
std::cout<<" "<<std::endl;
std::cout<<"-----------------------------------------------------------------"<<std::endl;
finish_motion_ = false;
finish_echo_robot_state_ = false;
}
/**
* @brief robot_driver_franka::_set_driver_mode sets the mode.
* @param mode
*/
void RobotInterfaceFranka::_set_driver_mode(const RobotInterfaceFranka::MODE& mode)
{
switch (mode)
{
case RobotInterfaceFranka::MODE::None:
mode_ = mode;
break;
case RobotInterfaceFranka::MODE::PositionControl:
mode_ = mode;
break;
case RobotInterfaceFranka::MODE::VelocityControl:
mode_ = mode;
break;
case RobotInterfaceFranka::MODE::ClearPositions:
throw std::runtime_error(std::string("The robot_driver_franka::ClearPositions is not available. "));
break;
case RobotInterfaceFranka::MODE::Homing:
throw std::runtime_error(std::string("The robot_driver_franka::Homing is not available. "));
break;
case RobotInterfaceFranka::MODE::ForceControl:
throw std::runtime_error(std::string("The robot_driver_franka::ForceControl is not available. "));
break;
}
}
/**
* @brief robot_driver_franka::connect starts the connection with the robot and starts the robot state thread.
*/
void RobotInterfaceFranka::connect()
{
_update_status_message("Connecting...", verbose_);
current_joint_positions_ = read_once_initial_positions(); //_read_once_smooth_initial_positions(1000);
desired_joint_positions_ = current_joint_positions_;
desired_joint_velocities_ = VectorXd::Zero(7);
_update_status_message("Connected.", verbose_);
_start_echo_robot_state_mode_thread(); // start_robot_state_loop to read joints without moving the robot;
}
/**
* @brief robot_driver_franka::disconnect stops all loops threads. Use this method after to run deinitialize().
*/
void RobotInterfaceFranka::disconnect()
{
_update_status_message("Disconnecting...", verbose_);
_finish_echo_robot_state();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
if (joint_position_control_mode_thread_.joinable())
{
joint_position_control_mode_thread_.join();
}
if (echo_robot_state_mode_thread_.joinable())
{
echo_robot_state_mode_thread_.join();
}
if (joint_velocity_control_mode_thread_.joinable())
{
joint_velocity_control_mode_thread_.join();
}
_update_status_message("Disconnected.", verbose_);
_restart_default_parameters();
}
void RobotInterfaceFranka::set_target_joint_velocities(const VectorXd &target_joint_velocities)
{
desired_joint_velocities_ = target_joint_velocities;
}
/**
* @brief robot_driver_franka::_restart_default_parameters
*/
void RobotInterfaceFranka::_restart_default_parameters()
{
finish_motion_ = false;
finish_echo_robot_state_ = false;
initialize_flag_ = false;
time_ = 0;
}
/**
* @brief robot_driver_franka::_compute_recursive_mean computes the recursive mean of a vector
* @param n The number of samples
* @param q The vector
* @return The recursive mean of n samples.
*/
VectorXd RobotInterfaceFranka::_compute_recursive_mean(const double& n, const VectorXd& q)
{
//Vector7d qm;
Vector7d qmean;
for (int i=0;i<q.size();i++)
{
//qm[i] = (1/n)*((n-1)*q[i] + q[i]);
qmean[i] = (1/n)*((n-1)*q[i] + q[i]);
}
return qmean;
}
/**
* @brief robot_driver_franka::_start_joint_position_control_thread
*/
void RobotInterfaceFranka::_start_joint_position_control_thread()
{
finish_motion_ = false;
_update_status_message("checking joint position control thread",verbose_);
if (joint_position_control_mode_thread_.joinable())
{
joint_position_control_mode_thread_.join();
}
_update_status_message("Starting joint position control thread",verbose_);
joint_position_control_mode_thread_ = std::thread(&RobotInterfaceFranka::_start_joint_position_control_mode, this);
}
void RobotInterfaceFranka::_start_joint_velocity_control_thread()
{
finish_motion_ = false;
_update_status_message("checking joint velocity control thread",verbose_);
if (joint_velocity_control_mode_thread_.joinable())
{
joint_velocity_control_mode_thread_.join();
}
_update_status_message("Starting joint velocity control thread",verbose_);
joint_velocity_control_mode_thread_ = std::thread(&RobotInterfaceFranka::_start_joint_velocity_control_mode, this);
}
/**
* @brief robot_driver_franka::_start_echo_robot_state_mode_thread
*/
void RobotInterfaceFranka::_start_echo_robot_state_mode_thread()
{
finish_echo_robot_state_ = false;
_update_status_message("Checking echo robot state thread",verbose_);
if (echo_robot_state_mode_thread_.joinable())
{
echo_robot_state_mode_thread_.join();
}
_update_status_message("Starting echo robot state thread",verbose_);
echo_robot_state_mode_thread_ = std::thread(&RobotInterfaceFranka::_start_echo_robot_state_mode, this);
}
/**
* @brief robot_driver_franka::finish_motion
*/
void RobotInterfaceFranka::finish_motion()
{
_update_status_message("Ending control loop.",verbose_);
for (int i=0;i<1000;i++)
{
set_target_joint_positions(current_joint_positions_);
set_target_joint_velocities(VectorXd::Zero(7));
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
finish_motion_ = true;
}
/**
* @brief robot_driver_franka::get_robot_pointer
* @return
*/
std::shared_ptr<franka::Robot> RobotInterfaceFranka::get_robot_pointer()
{
return robot_sptr_;
}
/**
* @brief robot_driver_franka::_finish_echo_robot_state
*/
void RobotInterfaceFranka::_finish_echo_robot_state()
{
_update_status_message("Finishing echo robot state.",verbose_);
finish_echo_robot_state_ = true;
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
/**
* @brief robot_driver_franka::deinitialize
*/
void RobotInterfaceFranka::deinitialize()
{
_update_status_message("Deinitializing....",verbose_);
initialize_flag_ = false;
switch (mode_)
{
case RobotInterfaceFranka::MODE::None:
break;
case RobotInterfaceFranka::MODE::PositionControl:
finish_motion();
break;
case RobotInterfaceFranka::MODE::VelocityControl:
finish_motion();
break;
case RobotInterfaceFranka::MODE::ClearPositions:
throw std::runtime_error(std::string("The robot_driver_franka::ClearPositions is not available. "));
break;
case RobotInterfaceFranka::MODE::Homing:
throw std::runtime_error(std::string("The robot_driver_franka::Homing is not available. "));
break;
case RobotInterfaceFranka::MODE::ForceControl:
throw std::runtime_error(std::string("The robot_driver_franka::ForceControl is not available. "));
break;
}
_update_status_message("Deinitialized.",verbose_);
}
/**
* @brief robot_driver_franka::initialize
*/
void RobotInterfaceFranka::initialize()
{
_update_status_message("Initializing....",verbose_);
initialize_flag_ = true;
switch (mode_)
{
case RobotInterfaceFranka::MODE::None:
break;
case RobotInterfaceFranka::MODE::PositionControl:
_finish_echo_robot_state();
_start_joint_position_control_thread();
break;
case RobotInterfaceFranka::MODE::VelocityControl:
_finish_echo_robot_state();
_start_joint_velocity_control_thread();
break;
case RobotInterfaceFranka::MODE::ClearPositions:
throw std::runtime_error(std::string("The robot_driver_franka::ClearPositions is not available. "));
break;
case RobotInterfaceFranka::MODE::Homing:
throw std::runtime_error(std::string("The robot_driver_franka::Homing is not available. "));
break;
case RobotInterfaceFranka::MODE::ForceControl:
throw std::runtime_error(std::string("The robot_driver_franka::ForceControl is not available. "));
break;
}
_update_status_message("Initialized.",verbose_);
}
/**
* @brief robot_driver_franka::_update_robot_state
* @param robot_state
* @param time
*/
void RobotInterfaceFranka::_update_robot_state(const franka::RobotState &robot_state, const double& time)
{
current_joint_positions_array_ = robot_state.q_d;
current_joint_positions_ = Eigen::Map<VectorXd>(current_joint_positions_array_.data(), 7);
current_joint_velocities_array_ = robot_state.dq_d;
current_joint_velocities_ = Eigen::Map<VectorXd>(current_joint_velocities_array_.data(), 7);
current_joint_forces_array_ = robot_state.tau_J;
current_joint_forces_ = Eigen::Map<VectorXd>(current_joint_forces_array_.data(), 7);
robot_mode_ = robot_state.robot_mode;
time_ = time;
}
/**
* @brief robot_driver_franka::_start_echo_robot_state_mode
*/
void RobotInterfaceFranka::_start_echo_robot_state_mode(){
double time = 0.0;
size_t count = 0;
double t1 = 0.0;
try {
robot_sptr_->read([&t1, &count, &time, this](const franka::RobotState& robot_state) {
if (count == 0)
{
t1 = robot_state.time.toSec();
}
time = robot_state.time.toSec() - t1;
_update_robot_state(robot_state, time);
count++;
return !finish_echo_robot_state_;
});
_update_status_message("Finished echo_robot_state.",verbose_);
}
catch (franka::Exception const& e) {
std::cout << e.what() << std::endl;
}
}
/**
* @brief robot_driver_franka::_start_joint_position_control_mode
*/
void RobotInterfaceFranka::_start_joint_position_control_mode()
{
std::array<double, 7> initial_position;
VectorXd target_position;
double time = 0.0;
VectorXd q = VectorXd::Zero(7);
franka::RobotState state = robot_sptr_->readOnce();
q = Map<VectorXd>(state.q_d.data(), 7);
desired_joint_positions_ = q;
VectorXd q_dot = VectorXd::Zero(7);
_update_status_message("Starting joint position control mode EXPERIMENTAL",verbose_);
trajectory_generator_sptr_ =
std::make_unique<QuadraticProgramMotionGenerator>(1.0, q, q_dot, q);
double n2 = 2;
double n1 = 1000*std::sqrt(n2);
VectorXd K2 = (VectorXd(7)<<n2, n2, n2, n2, n2, n2, n2).finished();
VectorXd K1 = (VectorXd(7)<<n1, n1, n1, n1, 3*n1, 3*n1, 3*n1).finished();
trajectory_generator_sptr_->set_diagonal_gains(K1, K2);
finish_motion_ = false;
int retry_counter = 0;
int TIMEOUT_IN_MILISECONDS = 5000;
try {
robot_sptr_->control( //------------------------------------------------------------
[&initial_position, &time, this](const franka::RobotState& robot_state,
franka::Duration period) -> franka::JointPositions {
time += period.toSec();
double T = period.toSec();
auto new_q = trajectory_generator_sptr_->compute_new_configuration(desired_joint_positions_, T);
if (time == 0.0) {
initial_position = robot_state.q_d;
new_q = Eigen::Map<VectorXd>(initial_position.data(), 7);
}
franka::JointPositions output = {{new_q[0], new_q[1],
new_q[2], new_q[3],
new_q[4], new_q[5],
new_q[6]}};
if (time == 0.0) {
_update_status_message("joint position control mode running! ",verbose_);
}
if (finish_motion_) {
_update_status_message("Motion finished",verbose_);
finish_motion_ = false;
return franka::MotionFinished(output);
}else
{
_update_robot_state(robot_state, time);
}
return output;
}
);//------------------------------------------------------------
}
catch (franka::Exception const& e) {
std::cout << e.what() << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(TIMEOUT_IN_MILISECONDS));
retry_counter++;
}
/**
* @brief RobotInterfaceFranka::_start_joint_velocity_control_mode
*/
void RobotInterfaceFranka::_start_joint_velocity_control_mode()
{
finish_motion_ = false;
std::array<double, 7> initial_velocity;
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);
double n2 = 1;
double n1 = 10*std::sqrt(n2);
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);
_update_status_message("Starting joint velocity control mode EXPERIMENTAL",verbose_);
try {
double time = 0.0;
robot_sptr_->control(
[&initial_velocity, &time, this](const franka::RobotState& robot_state, franka::Duration period) -> franka::JointVelocities {
time += period.toSec();
double T = period.toSec();
auto new_u = trajectory_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],
new_u[4], new_u[5],
new_u[6]}};
if (time == 0.0) {
_update_status_message("joint velocity control mode running! ",verbose_);
}
if (finish_motion_) {
_update_status_message("Motion finished",verbose_);
finish_motion_ = false;
return franka::MotionFinished(velocities);
}else
{
_update_robot_state(robot_state, time);
initial_velocity = robot_state.dq_d;
}
return velocities;
});
}
catch (franka::Exception const& e) {
std::cout << e.what() << std::endl;
}
}
/**
* @brief robot_driver_franka::_setDefaultRobotBehavior
*/
void RobotInterfaceFranka::_setDefaultRobotBehavior()
{
robot_sptr_->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_sptr_->setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
robot_sptr_->setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
}
/**
* @brief robot_driver_franka::_update_status_message
* @param message
* @param verbose
*/
void RobotInterfaceFranka::_update_status_message(const std::string &message, const bool& verbose)
{
status_message_ = message;
std::string base = "RobotInterfaceFranka::";
if(verbose)
{
std::cout<<base+message<<std::endl;
}
}
/**
* @brief robot_driver_franka::get_status_message
* @return
*/
std::string RobotInterfaceFranka::get_status_message()
{
return status_message_;
}
/**
* @brief RobotInterfaceFranka::get_robot_mode
* @return
*/
std::string RobotInterfaceFranka::get_robot_mode()
{
std::string string_robot_mode;
switch (robot_mode_) {
case (franka::RobotMode::kUserStopped):
string_robot_mode = "User stopped";
break;
case (franka::RobotMode::kIdle):
string_robot_mode = "Idle";
break;
case (franka::RobotMode::kMove):
string_robot_mode = "Move";
break;
case (franka::RobotMode::kGuiding):
string_robot_mode = "Guiding";
break;
case (franka::RobotMode::kReflex):
string_robot_mode = "Reflex";
break;
case (franka::RobotMode::kAutomaticErrorRecovery):
string_robot_mode = "Automatic error recovery";
break;
case (franka::RobotMode::kOther):
string_robot_mode = "Other";
break;
}
return string_robot_mode;
}
/**
* @brief robot_driver_franka::_check_if_robot_is_connected
*/
void RobotInterfaceFranka::_check_if_robot_is_connected()
{
if(!robot_sptr_)
throw std::runtime_error("Invalid robot pointer. You must connect(), then initialize(). "
+ std::string("Example: robot_driver_franka(IP, robot_driver_franka::MODE::None, robot_driver_franka::HAND::ON)" ));
}
/**
* @brief robot_driver_franka::_check_if_hand_is_connected
*/
void RobotInterfaceFranka::_check_if_hand_is_connected()
{
if(!gripper_sptr_)
throw std::runtime_error("Invalid hand pointer. You must connect(), then initialize(). "
+ std::string("Example: robot_driver_franka(IP, robot_driver_franka::MODE::None, robot_driver_franka::HAND::ON)" ));
}
/**
* @brief robot_driver_franka::get_joint_positions
* @return
*/
VectorXd RobotInterfaceFranka::get_joint_positions()
{
_check_if_robot_is_connected();
return current_joint_positions_;
}
/**
* @brief robot_driver_franka::get_joint_velocities
* @return
*/
VectorXd RobotInterfaceFranka::get_joint_velocities()
{
_check_if_robot_is_connected();
return current_joint_velocities_;
}
/**
* @brief RobotInterfaceFranka::get_joint_forces
* @return
*/
VectorXd RobotInterfaceFranka::get_joint_forces()
{
_check_if_robot_is_connected();
return current_joint_forces_;
}
/**
* @brief robot_driver_franka::get_time
* @return
*/
double RobotInterfaceFranka::get_time()
{
_check_if_robot_is_connected();
return time_;
}
/**
* @brief robot_driver_franka::_read_once_smooth_initial_positions
* @param samples
* @return
*/
VectorXd RobotInterfaceFranka::_read_once_smooth_initial_positions(const double& samples)
{
_check_if_robot_is_connected();
VectorXd q = VectorXd::Zero(7);
_update_status_message("Reading smooth initial joints...",verbose_);
for (int i=0;i<samples;i++ )
{
franka::RobotState state = robot_sptr_->readOnce();
q = Map<VectorXd>(state.q_d.data(), 7);
q = _compute_recursive_mean(samples, q);
}
return q;
}
/**
* @brief robot_driver_franka::read_once_initial_positions
* @return
*/
VectorXd RobotInterfaceFranka::read_once_initial_positions()
{
_check_if_robot_is_connected();
VectorXd q = VectorXd::Zero(7);
_update_status_message("Reading initial joints...",verbose_);
franka::RobotState state = robot_sptr_->readOnce();
q = Map<VectorXd>(state.q_d.data(), 7);
return q;
}
/**
* @brief robot_driver_franka::move_robot_to_target_joint_positions
* @param q_target
*/
void RobotInterfaceFranka::move_robot_to_target_joint_positions(const VectorXd& q_target)
{
_finish_echo_robot_state();
_check_if_robot_is_connected();
if (!initialize_flag_)
{
throw std::runtime_error(std::string("Error in set_initial_robot_configuration. Driver not initialized. Use initialize(); "));
}
MotionGenerator motion_generator(speed_factor_joint_position_controller_, q_target);
try {
_update_status_message("Moving robot...", verbose_);
robot_sptr_->control(motion_generator);
}
catch (franka::Exception const& e) {
std::cout << e.what() << std::endl;
}
_start_echo_robot_state_mode_thread();
}
/**
* @brief robot_driver_franka::set_joint_target_positions
* @param set_target_joint_positions_rad
*/
void RobotInterfaceFranka::set_target_joint_positions(const VectorXd& set_target_joint_positions_rad)
{
desired_joint_positions_ = set_target_joint_positions_rad;
}
/**
* @brief robot_driver_franka::get_joint_target_positions
* @return
*/
VectorXd RobotInterfaceFranka::get_joint_target_positions()
{
return desired_joint_positions_;
}
/**
* @brief robot_driver_franka::read_gripper
* @param gripper_state
* @return
*/
double RobotInterfaceFranka::read_gripper(const GRIPPER_STATES& gripper_state)
{
_check_if_hand_is_connected();
franka::GripperState state = gripper_sptr_->readOnce();
switch (gripper_state) {
case RobotInterfaceFranka::GRIPPER_STATES::WIDTH:
return state.width;
break;
case RobotInterfaceFranka::GRIPPER_STATES::MAX_WIDTH:
return state.max_width;
break;
default:
throw std::runtime_error(std::string("Wrong argument in sas_robot_driver_franka::read_gripper. "));
break;
}
}
/**
* @brief robot_driver_franka::read_grasped_status
* @return
*/
bool RobotInterfaceFranka::read_grasped_status()
{
_check_if_hand_is_connected();
franka::GripperState state = gripper_sptr_->readOnce();
return state.is_grasped;
}
/**
* @brief robot_driver_franka::set_gripper
* @param width
*/
void RobotInterfaceFranka::set_gripper(const double& width)
{
_check_if_hand_is_connected();
auto gripper_max_width = read_gripper(RobotInterfaceFranka::GRIPPER_STATES::MAX_WIDTH);
if (width > gripper_max_width)
{
throw std::runtime_error(
std::string("You used a width = ") +
std::to_string(width) +
std::string(". Maximum width allowed is ") +
std::to_string(gripper_max_width)
);
}
gripper_sptr_->move(width, speed_gripper_);
}
/**
* @brief robot_driver_franka::gripper_homing
*/
void RobotInterfaceFranka::gripper_homing()
{
_check_if_hand_is_connected();
gripper_sptr_->homing();
}
/**
* @brief robot_driver_franka::get_home_robot_configuration
* @return
*/
VectorXd RobotInterfaceFranka::get_home_robot_configuration()
{
return q_home_configuration_;
}

120
src/sas_robot_driver_franka.cpp Normal file
View File

@@ -0,0 +1,120 @@
#include "sas_robot_driver_franka.h"
#include "sas_clock/sas_clock.h"
#include <dqrobotics/utils/DQ_Math.h>
namespace sas
{
RobotDriverFranka::RobotDriverFranka(const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
RobotDriver(break_loops),
configuration_(configuration)
{
joint_positions_.resize(7);
joint_velocities_.resize(7);
joint_forces_.resize(7);
end_effector_pose_.resize(7);
joint_positions_buffer_.resize(8,0);
end_effector_pose_euler_buffer_.resize(7,0);
end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
std::cout<<configuration.ip_address<<std::endl;
robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(configuration.ip_address,
RobotInterfaceFranka::MODE::VelocityControl, //None, PositionControl
RobotInterfaceFranka::HAND::ON);
}
RobotDriverFranka::~RobotDriverFranka()=default;
/**
* @brief
*
*/
void RobotDriverFranka::connect()
{
robot_driver_interface_sptr_ ->connect();
}
/**
* @brief
*
*/
void RobotDriverFranka::initialize()
{
robot_driver_interface_sptr_->initialize();
}
/**
* @brief
*
*/
void RobotDriverFranka::deinitialize()
{
robot_driver_interface_sptr_->deinitialize();
}
/**
* @brief
*
*/
void RobotDriverFranka::disconnect()
{
robot_driver_interface_sptr_->disconnect();
}
/**
* @brief
*
* @return VectorXd
*/
VectorXd RobotDriverFranka::get_joint_positions()
{
return robot_driver_interface_sptr_->get_joint_positions();
}
/**
* @brief
*
* @param desired_joint_positions_rad
*/
void RobotDriverFranka::set_target_joint_positions(const VectorXd& desired_joint_positions_rad)
{
robot_driver_interface_sptr_->set_target_joint_positions(desired_joint_positions_rad);
}
/**
* @brief RobotDriverFranka::get_joint_velocities
* @return
*/
VectorXd RobotDriverFranka::get_joint_velocities()
{
joint_velocities_ = robot_driver_interface_sptr_->get_joint_velocities();
return joint_velocities_;
}
/**
* @brief RobotDriverFranka::set_target_joint_velocities
* @param desired_joint_velocities
*/
void RobotDriverFranka::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
{
desired_joint_velocities_ = desired_joint_velocities;
robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities);
}
/**
* @brief RobotDriverFranka::get_joint_forces
* @return
*/
VectorXd RobotDriverFranka::get_joint_forces()
{
joint_forces_ = robot_driver_interface_sptr_->get_joint_forces();
return joint_forces_;
}
}

90
src/sas_robot_driver_franka_node.cpp Normal file
View File

@@ -0,0 +1,90 @@
//#include "ros/ros.h"
//#include "std_msgs/String.h"
#include <sstream>
#include <exception>
#include <dqrobotics/utils/DQ_Math.h>
//#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
#include <sas_common/sas_common.h>
#include <sas_conversions/eigen3_std_conversions.h>
#include <sas_robot_driver/sas_robot_driver_ros.h>
#include "sas_robot_driver_franka.h"
/*********************************************
* SIGNAL HANDLER
* *******************************************/
#include<signal.h>
static std::atomic_bool kill_this_process(false);
void sig_int_handler(int)
{
kill_this_process = true;
}
int main(int argc, char **argv)
{
if(signal(SIGINT, sig_int_handler) == SIG_ERR)
{
throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
}
ros::init(argc, argv, "sas_robot_driver_franka_node", ros::init_options::NoSigintHandler);
ROS_WARN("=====================================================================");
ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------");
ROS_WARN("=====================================================================");
ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
ros::NodeHandle nh;
sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address);
// sas::get_ros_param(nh,"/robot_port", robot_driver_franka_configuration.port);
//sas::get_ros_param(nh,"/robot_speed", robot_driver_franka_configuration.speed);
//robot_driver_franka_configuration.ip_address = "172.16.0.2";
//robot_driver_franka_configuration.port = 0;
//robot_driver_franka_configuration.speed = 0.0;
sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
//auto qmin = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_min);
//auto qmax = sas::std_vector_double_to_vectorxd(robot_driver_ros_configuration.q_max);
//std::vector<double> q_min = {-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895};
//std::vector<double> q_max = { 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895};
//robot_driver_ros_configuration.thread_sampling_time_nsec = 4000000;
//robot_driver_ros_configuration.q_min = q_min;
//robot_driver_ros_configuration.q_max = q_max;
robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
try
{
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka robot.");
sas::RobotDriverFranka robot_driver_franka(robot_driver_franka_configuration,
&kill_this_process);
//robot_driver_franka.set_joint_limits({qmin, qmax});
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
sas::RobotDriverROS robot_driver_ros(nh,
&robot_driver_franka,
robot_driver_ros_configuration,
&kill_this_process);
robot_driver_ros.control_loop();
}
catch (const std::exception& e)
{
ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
}
return 0;
}

235
src/teleop_franka_node.cpp Normal file
View File

@@ -0,0 +1,235 @@
#include <ros/ros.h>
#include <sas_common/sas_common.h>
#include <sas_robot_kinematics/sas_robot_kinematics_provider.h>
#include <sas_robot_driver/sas_robot_driver_interface.h>
#include <iostream>
#include <Eigen/Dense>
#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
//#include <dqrobotics/robots/FrankaEmikaPandaMDHRobot.h>
#include <dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h>
//#include <dqrobotics/solvers/DQ_SerialManipulatorDynamicsGaussPrincipleSolver.h>
#include <dqrobotics/solvers/DQ_QuadraticProgrammingSolver.h>
#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
#include <dqrobotics/interfaces/vrep/robots/FrankaEmikaPandaVrepRobot.h>
#include <thread>
#include <dqrobotics/DQ.h>
#include <dqrobotics/solvers/DQ_QPOASESSolver.h>
#include <dqrobotics/robot_control/DQ_ClassicQPController.h>
#include <chrono>
#include <yaml-cpp/yaml.h>
#include <RobotConstraintsManager.h>
#include "franka_ros_interface.h"
#include <QuadraticProgramMotionGenerator.h>
std::string config_vfi_path = "/home/moonshot/Documents/git/franka_emika/real_franka_developments/real_franka_developments/cfg/vfi_constraints.yaml";
int main(int argc, char **argv)
{
ros::init(argc, argv, "teleop_franka_node");
auto nodehandle = ros::NodeHandle();
sas::RobotKinematicsProvider pose1_provider(nodehandle, "/arm1_kinematics");
//auto franka1_ros = FrankaRosInterface(nodehandle, 50, "/franka_1");
sas::RobotDriverInterface franka1_ros(nodehandle, "/franka_1");
//##########################################################################################
DQ_VrepInterface vi;
vi.connect("10.198.113.159", 19997,100,10);
std::cout << "Starting V-REP simulation..." << std::endl;
vi.start_simulation();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
auto franka_sim = FrankaEmikaPandaVrepRobot("Franka", std::make_shared<DQ_VrepInterface>(vi));
auto robot = franka_sim.kinematics();
DQ xoffset = (1 + E_*0.5*0.21*k_)*(cos((M_PI/4)/2)-k_*sin((M_PI/4)/2));
robot.set_effector(xoffset);
//###########################################################################################
auto manager = RobotConstraintsManager(std::make_shared<DQ_VrepInterface>(vi),
(std::static_pointer_cast<DQ_Kinematics>(std::make_shared<DQ_SerialManipulatorMDH>(robot))),
(std::static_pointer_cast<DQ_VrepRobot>(std::make_shared<FrankaEmikaPandaVrepRobot>(franka_sim))),
config_vfi_path,
RobotConstraintsManager::ORDER::FIRST_ORDER);
//##########################################################################################
//#######------------------------Controller------------------------------------------#######
DQ_QPOASESSolver solver;
DQ_ClassicQPController controller
(std::static_pointer_cast<DQ_Kinematics>(std::make_shared<DQ_SerialManipulatorMDH>(robot)),
std::static_pointer_cast<DQ_QuadraticProgrammingSolver>(std::make_shared<DQ_QPOASESSolver>(solver)));
controller.set_gain(20.0);
controller.set_damping(0.01);
controller.set_control_objective(DQ_robotics::Translation); //DistanceToPlane Translation
controller.set_stability_threshold(0.0001);
//##########################################################################################
//#########################################################################################
DQ xd;
DQ x;
MatrixXd A;
VectorXd b;
VectorXd u;
VectorXd q_u = VectorXd::Zero(7);
VectorXd q;
VectorXd q_dot_initial = VectorXd::Zero(7);
VectorXd q_dot = VectorXd::Zero(7);
while (ros::ok()) {
if (franka1_ros.is_enabled())
{
q = franka1_ros.get_joint_positions();
break;
}
ros::spinOnce();
}
q_u = q;
franka_sim.set_target_configuration_space_positions(q);
vi.set_object_pose("xd", robot.fkm(q));
/*
DQ_QPOASESSolver solver2;
MatrixXd H = MatrixXd::Identity(7, 7);
VectorXd f ;
VectorXd qvrep;
MatrixXd A_;
VectorXd b_;
for (int i=0;i<3000;i++)
{
qvrep = franka_sim.get_configuration_space_positions();
f = 2*0.1*(qvrep-q);
auto u_ = solver2.solve_quadratic_program(H, f, A_, b_, A_, b_);
franka_sim.set_target_configuration_space_velocities(u_);
std::cout<<"initialiazing..."<<std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
*/
std::cout<<"-------------------------"<<std::endl;
std::cout<<"Initial q: "<<std::endl;
std::cout<<q.transpose()<<std::endl;
std::cout<<"-------------------------"<<std::endl;
double T = 0.001;
try {
while(ros::ok())
{
if (q.size() == 7)
{
q = franka1_ros.get_joint_positions();
franka_sim.set_target_configuration_space_positions(q);
//q = franka_sim.get_configuration_space_positions();
x = robot.fkm(q);
vi.set_object_pose("effector", x);
if(not pose1_provider.is_enabled())
{
pose1_provider.send_pose(x);
}
pose1_provider.send_reference_frame(robot.get_reference_frame());
try{
xd = pose1_provider.get_desired_pose();
pose1_provider.send_pose(xd);
}
catch(const std::exception& e){
xd = vi.get_object_pose("xd");
std::cout<<"Nope"<<std::endl;
std::cout << e.what() << std::endl;
}
xd = vi.get_object_pose("xd");
std::tie(A, b) = manager.get_inequality_constraints(q);
controller.set_inequality_constraint(A,b);
//-----------------------------------------------------------
switch(controller.get_control_objective())
{
case ControlObjective::Distance:
{
VectorXd pdesired = vec4(translation(xd));
VectorXd distance(1);
distance(0) = pdesired.transpose()*pdesired;
u = controller.compute_setpoint_control_signal(q, pdesired.transpose()*pdesired);
break;
}
case ControlObjective::Translation:
{
u = controller.compute_setpoint_control_signal(q, vec4(xd.translation()));
break;
}
case ControlObjective::Pose:
{
u = controller.compute_setpoint_control_signal(q, vec8(xd));
break;
}
case ControlObjective::DistanceToPlane:
{
DQ plane_normal = xd.P()*k_*xd.P().conj();
DQ plane_point = xd.translation();
DQ xdesired_plane = (plane_normal + E_ * dot(plane_point, plane_normal));
controller.set_target_primitive(xdesired_plane);
u = controller.compute_setpoint_control_signal(q, VectorXd::Zero(1)); //DistanceToPlane
break;
}
default:
throw std::runtime_error("ControlObjective not suppported.");
break;
}
//q = q + T*u;
//std::cout<<"error: "<<std::endl;
//std::cout<<controller.get_last_error_signal().norm()<<std::endl;
//franka_sim.set_target_configuration_space_positions(franka1_ros.get_joint_positions());
//auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(u, T);
//franka_sim.set_target_configuration_space_velocities(new_u);
//franka1_ros.send_target_joint_positions(new_u);
//auto new_u = trajectory_generator_sptr_->compute_new_configuration_velocities(u, T);
//franka1_ros.send_target_joint_positions(q);
franka1_ros.send_target_joint_velocities(u);
}
ros::spinOnce();
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::cout << "Stopping V-REP simulation..." << std::endl;
vi.stop_simulation();
vi.disconnect();
return 0;
}
catch (const std::exception& e) {
vi.stop_simulation();
vi.disconnect();
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::cout << e.what() << std::endl;
return -1;
}
}