Echo contact (#1)
* added cartesian interface * general fix and optimization
This commit is contained in:
qlin960618
GitHub
@@ -1,213 +0,0 @@
|
||||
/*
|
||||
# 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 <dqrobotics/DQ.h>
|
||||
#include <memory.h>
|
||||
#include <tuple>
|
||||
#include <exception>
|
||||
#include <vector>
|
||||
#include <franka/robot.h>
|
||||
#include <franka/gripper.h>
|
||||
#include <franka/exception.h>
|
||||
#include "motion_generator.h"
|
||||
#include <thread>
|
||||
#include "quadratic_program_motion_generator.h"
|
||||
#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
|
||||
#include <atomic>
|
||||
#include "custom_motion_generation.h"
|
||||
|
||||
using namespace DQ_robotics;
|
||||
using namespace Eigen;
|
||||
|
||||
class RobotInterfaceFranka
|
||||
{
|
||||
public: enum class MODE{
|
||||
None=0,
|
||||
PositionControl,
|
||||
VelocityControl,
|
||||
ForceControl,
|
||||
Homing,
|
||||
ClearPositions,
|
||||
};
|
||||
|
||||
private:
|
||||
using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
|
||||
std::string ip_ = "172.16.0.2";
|
||||
double speed_factor_joint_position_controller_ = 0.2;
|
||||
double speed_gripper_ = 0.02;
|
||||
VectorXd desired_joint_positions_;
|
||||
VectorXd desired_joint_velocities_ = VectorXd::Zero(7);
|
||||
|
||||
VectorXd current_joint_positions_;
|
||||
std::array<double, 7> current_joint_positions_array_;
|
||||
|
||||
VectorXd current_joint_velocities_;
|
||||
std::array<double, 7> current_joint_velocities_array_;
|
||||
|
||||
VectorXd current_joint_forces_;
|
||||
std::array<double, 7> current_joint_forces_array_;
|
||||
|
||||
franka::RobotMode robot_mode_;
|
||||
|
||||
double time_ = 0;
|
||||
bool initialize_flag_ = false;
|
||||
void _check_if_robot_is_connected();
|
||||
void _check_if_hand_is_connected();
|
||||
|
||||
RobotInterfaceFranka::MODE mode_;
|
||||
void _set_driver_mode(const RobotInterfaceFranka::MODE& mode);
|
||||
|
||||
void _restart_default_parameters();
|
||||
|
||||
|
||||
void _update_robot_state(const franka::RobotState& robot_state, const double& time);
|
||||
|
||||
std::shared_ptr<franka::Robot> robot_sptr_;
|
||||
std::shared_ptr<franka::Gripper> gripper_sptr_;
|
||||
std::unique_ptr<QuadraticProgramMotionGenerator> trajectory_generator_sptr_;
|
||||
std::unique_ptr<CustomMotionGeneration> custom_generator_sptr_;
|
||||
|
||||
void _setDefaultRobotBehavior();
|
||||
//bool hand_enabled_ = false;
|
||||
const VectorXd q_home_configuration_ =(VectorXd(7)<<0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4).finished();
|
||||
|
||||
const VectorXd q_max_ = ((VectorXd(7) << 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895).finished());
|
||||
const VectorXd q_min_ = ((VectorXd(7) << -2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895).finished());
|
||||
const VectorXd q_min_dot_ = ((VectorXd(7) << -2, -1, -1.5, -1.25, -3, -1.5, -3).finished());
|
||||
const VectorXd q_max_dot_ = ((VectorXd(7) << 2, 1, 1.5, 1.25, 3, 1.5, 3).finished());
|
||||
|
||||
const double samples_ = 20;
|
||||
|
||||
VectorXd desired_mean_joint_positions_;
|
||||
|
||||
bool verbose_ = true;
|
||||
std::string status_message_ = " ";
|
||||
void _update_status_message(const std::string& message, const bool& verbose = true);
|
||||
|
||||
|
||||
//-------To handle the threads-----------------
|
||||
std::atomic<bool> exit_on_err_={false};
|
||||
|
||||
|
||||
void _start_joint_position_control_mode();
|
||||
std::thread joint_position_control_mode_thread_;
|
||||
void _start_joint_position_control_thread();
|
||||
std::atomic<bool> finish_motion_;
|
||||
|
||||
void _start_echo_robot_state_mode();
|
||||
std::thread echo_robot_state_mode_thread_;
|
||||
void _start_echo_robot_state_mode_thread();
|
||||
std::atomic<bool> finish_echo_robot_state_;
|
||||
void _finish_echo_robot_state();
|
||||
|
||||
void _start_joint_velocity_control_mode();
|
||||
std::thread joint_velocity_control_mode_thread_;
|
||||
void _start_joint_velocity_control_thread();
|
||||
//----------------------------------------------
|
||||
|
||||
|
||||
|
||||
VectorXd _compute_recursive_mean(const double& samples, const VectorXd& q);
|
||||
VectorXd _read_once_smooth_initial_positions(const double& samples);
|
||||
|
||||
|
||||
public:
|
||||
|
||||
|
||||
enum HAND{ON=0, OFF};
|
||||
enum CONNECTION{AUTOMATIC};
|
||||
enum GRIPPER_STATES{WIDTH=0, MAX_WIDTH=1};
|
||||
RobotInterfaceFranka() = delete;
|
||||
RobotInterfaceFranka(const RobotInterfaceFranka&) = delete;
|
||||
RobotInterfaceFranka& operator= (const RobotInterfaceFranka&) = delete;
|
||||
RobotInterfaceFranka(const std::string &ROBOT_IP, const MODE& mode, const HAND& hand = ON);
|
||||
|
||||
|
||||
|
||||
VectorXd read_once_initial_positions();
|
||||
|
||||
VectorXd get_joint_target_positions();
|
||||
|
||||
void move_robot_to_target_joint_positions(const VectorXd& q_target);
|
||||
|
||||
|
||||
|
||||
|
||||
double read_gripper(const GRIPPER_STATES& gripper_state = WIDTH);
|
||||
bool read_grasped_status();
|
||||
void set_gripper(const double& width);
|
||||
void gripper_homing();
|
||||
VectorXd get_home_robot_configuration();
|
||||
|
||||
std::string get_status_message();
|
||||
|
||||
bool get_err_state() const {return exit_on_err_.load();}
|
||||
|
||||
std::string get_robot_mode();
|
||||
|
||||
|
||||
void finish_motion();
|
||||
|
||||
std::shared_ptr<franka::Robot> get_robot_pointer();
|
||||
|
||||
|
||||
//--------sas compatible methods----------//
|
||||
VectorXd get_joint_positions();
|
||||
VectorXd get_joint_velocities();
|
||||
VectorXd get_joint_forces();
|
||||
|
||||
double get_time();
|
||||
|
||||
void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad);
|
||||
void connect();
|
||||
void initialize();
|
||||
void deinitialize();
|
||||
void disconnect();
|
||||
|
||||
void set_target_joint_velocities(const VectorXd& target_joint_velocities);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
/* To be implemented
|
||||
std::tuple<VectorXd, VectorXd> get_joint_limits() const;
|
||||
void set_joint_limits(const std::tuple<VectorXd, VectorXd>& joint_limits);
|
||||
*/
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
@@ -1,134 +0,0 @@
|
||||
/*
|
||||
# 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)
|
||||
# - Adapted from sas_robot_driver_denso.cpp
|
||||
# (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso.cpp)
|
||||
#
|
||||
# ################################################################
|
||||
*/
|
||||
|
||||
|
||||
#pragma once
|
||||
#include <exception>
|
||||
#include <tuple>
|
||||
#include <atomic>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <dqrobotics/DQ.h>
|
||||
#include <sas_robot_driver/sas_robot_driver.h>
|
||||
#include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
|
||||
#include <thread>
|
||||
#include <sas_robot_driver/sas_robot_driver_interface.h>
|
||||
|
||||
using namespace DQ_robotics;
|
||||
using namespace Eigen;
|
||||
|
||||
namespace sas
|
||||
{
|
||||
|
||||
|
||||
|
||||
struct RobotDriverCoppeliaConfiguration
|
||||
{
|
||||
|
||||
int thread_sampling_time_nsec;
|
||||
int port;
|
||||
std::string ip;
|
||||
std::vector<std::string> jointnames;
|
||||
std::string robot_mode;
|
||||
bool mirror_mode;
|
||||
std::string real_robot_topic_prefix;
|
||||
};
|
||||
|
||||
class RobotDriverCoppelia: public RobotDriver
|
||||
{
|
||||
private:
|
||||
RobotDriverCoppeliaConfiguration configuration_;
|
||||
|
||||
std::string robot_mode_ = std::string("VelocityControl"); // PositionControl
|
||||
bool mirror_mode_ = false;
|
||||
double gain_ = 3.0;
|
||||
std::string real_robot_topic_prefix_;
|
||||
|
||||
VectorXd current_joint_positions_;
|
||||
VectorXd current_joint_velocities_;
|
||||
VectorXd current_joint_forces_;
|
||||
|
||||
|
||||
VectorXd desired_joint_velocities_;
|
||||
VectorXd desired_joint_positions_;
|
||||
|
||||
std::atomic<bool> finish_motion_;
|
||||
|
||||
int dim_configuration_space_;
|
||||
|
||||
void _update_robot_state(const VectorXd& q, const VectorXd& q_dot, const VectorXd& forces);
|
||||
|
||||
void finish_motion();
|
||||
|
||||
void _start_joint_velocity_control_mode();
|
||||
std::thread joint_velocity_control_mode_thread_;
|
||||
void _start_joint_velocity_control_thread();
|
||||
|
||||
|
||||
void _start_joint_velocity_control_mirror_mode();
|
||||
std::thread joint_velocity_control_mirror_mode_thread_;
|
||||
void _start_joint_velocity_control_mirror_thread();
|
||||
|
||||
std::shared_ptr<sas::RobotDriverInterface> franka1_ros_;
|
||||
|
||||
|
||||
protected:
|
||||
std::shared_ptr<DQ_VrepInterface> vi_;
|
||||
std::vector<std::string> jointnames_;
|
||||
|
||||
|
||||
|
||||
public:
|
||||
RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;
|
||||
RobotDriverCoppelia()=delete;
|
||||
~RobotDriverCoppelia();
|
||||
|
||||
RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
|
||||
|
||||
|
||||
VectorXd get_joint_positions() override;
|
||||
void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override;
|
||||
|
||||
VectorXd get_joint_velocities() override;
|
||||
void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override;
|
||||
|
||||
VectorXd get_joint_forces() override;
|
||||
|
||||
void connect() override;
|
||||
void disconnect() override;
|
||||
|
||||
void initialize() override;
|
||||
void deinitialize() override;
|
||||
|
||||
|
||||
};
|
||||
}
|
||||
@@ -43,20 +43,23 @@
|
||||
#include <sas_robot_driver/sas_robot_driver.h>
|
||||
#include "robot_interface_franka.h"
|
||||
#include <ros/common.h>
|
||||
#include "sas_robot_dynamic_provider.h"
|
||||
|
||||
using namespace DQ_robotics;
|
||||
using namespace Eigen;
|
||||
|
||||
struct RobotInterfaceFranka::FrankaInterfaceConfiguration; // Forward declaration
|
||||
|
||||
namespace sas
|
||||
{
|
||||
|
||||
|
||||
struct RobotDriverFrankaConfiguration
|
||||
{
|
||||
std::string ip_address;
|
||||
std::string mode;
|
||||
int port;
|
||||
double speed;
|
||||
RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration;
|
||||
};
|
||||
|
||||
|
||||
@@ -67,6 +70,8 @@ private:
|
||||
|
||||
std::shared_ptr<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;
|
||||
|
||||
RobotDynamicProvider* robot_dynamic_provider_;
|
||||
|
||||
//Joint positions
|
||||
VectorXd joint_positions_;
|
||||
//VectorXd joint_velocities_;
|
||||
@@ -77,6 +82,9 @@ private:
|
||||
|
||||
std::atomic_bool* break_loops_;
|
||||
|
||||
// hotfix function to update cartesian contact and pose information
|
||||
void _update_stiffness_contact_and_pose() const;
|
||||
|
||||
|
||||
public:
|
||||
//const static int SLAVE_MODE_JOINT_CONTROL;
|
||||
@@ -86,7 +94,11 @@ public:
|
||||
RobotDriverFranka()=delete;
|
||||
~RobotDriverFranka();
|
||||
|
||||
RobotDriverFranka(const RobotDriverFrankaConfiguration& configuration, std::atomic_bool* break_loops);
|
||||
RobotDriverFranka(
|
||||
RobotDynamicProvider* robot_dynamic_provider,
|
||||
const RobotDriverFrankaConfiguration& configuration,
|
||||
std::atomic_bool* break_loops
|
||||
);
|
||||
|
||||
|
||||
VectorXd get_joint_positions() override;
|
||||
|
||||
Reference in New Issue
Block a user