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

bug fix and tested coppelia mirror node

Browse Source
This commit is contained in:
QuentinLin
2024-08-14 14:25:40 +09:00
parent 7164bdf59d
commit 0b84820497
4 changed files with 77 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@@ -158,6 +158,7 @@ add_dependencies(qros_effector_driver_franka_hand ${catkin_EXPORTED_TARGETS})
add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp)
target_link_libraries(sas_robot_driver_coppelia
dqrobotics
dqrobotics-interface-json11
dqrobotics-interface-vrep)
add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

55
src/coppelia/sas_robot_driver_coppelia.cpp
View File

@@ -4,16 +4,21 @@
namespace sas
{
RobotDriverCoppelia::~RobotDriverCoppelia() {
deinitialize();
disconnect();
}
RobotDriverCoppelia::RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
configuration_(configuration),
clock_(configuration.thread_sampling_time_nsec),
break_loops_(break_loops),
robot_mode_(ControlMode::Position),
vi_(std::make_shared<DQ_VrepInterface>()),
joint_limits_(configuration.q_min, configuration.q_max)
vi_(std::make_shared<DQ_VrepInterface>())
{
// should initialize robot driver interface to real robot
DQ_SerialManipulatorDH smdh = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(configuration_.robot_parameter_file_path);
joint_limits_ = {smdh.get_lower_q_limit(),smdh.get_upper_q_limit()};
if(configuration_.using_real_robot)
{
ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using real robot, Instantiating robot interface to real driver.");
@@ -34,23 +39,27 @@ RobotDriverCoppelia::RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCo
}
void RobotDriverCoppelia::_update_vrep_position(VectorXd joint_positions){
void RobotDriverCoppelia::_update_vrep_position(const VectorXd &joint_positions, const bool& force_update) const{
if(configuration_.vrep_dynamically_enabled){
vi_->set_joint_target_positions(configuration_.vrep_robot_joint_names, joint_positions);
if(force_update){
vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions);
}
vi_->set_joint_target_positions(configuration_.vrep_joint_names, joint_positions);
}else{
vi_->set_joint_positions(configuration_.vrep_robot_joint_names, joint_positions);
vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions);
}
}
void RobotDriverCoppelia::_update_vrep_velocity(VectorXd joint_velocity){
void RobotDriverCoppelia::_update_vrep_velocity(const VectorXd & joint_velocity) const{
if(!configuration_.vrep_dynamically_enabled){
throw std::runtime_error("[RobotDriverCoppelia]::[_update_vrep_velocity]::Vrep is not dynamically enabled");
}
vi_->set_joint_target_velocities(configuration_.vrep_robot_joint_names, joint_velocity);
vi_->set_joint_target_velocities(configuration_.vrep_joint_names, joint_velocity);
}
void RobotDriverCoppelia::_start_control_loop(){
clock_.init();
ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Starting control loop...");
while(!_should_shutdown()){
clock_.update_and_sleep();
ros::spinOnce();
@@ -62,22 +71,26 @@ void RobotDriverCoppelia::_start_control_loop(){
}else{
// robot_provider_
VectorXd target_joint_positions;
auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_robot_joint_names);
auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_joint_names);
VectorXd current_joint_velocity;
if(robot_provider_->is_enabled()) {
if(robot_mode_ == ControlMode::Velocity)
{
if(robot_provider_->is_enabled(sas::RobotDriver::Functionality::VelocityControl)) {
simulated_joint_velocities_ = robot_provider_->get_target_joint_velocities();
current_joint_velocity = simulated_joint_velocities_;
// try{_update_vrep_velocity(simulated_joint_velocities_);}catch (...){}
// try{_update_vrep_velocity(simulated_joint_velocities_);}catch (...){}
}
else{
ROS_WARN_STREAM("[" + ros::this_node::getName() + "]::Velocity control is not enabled.");
ROS_DEBUG_STREAM("[" + ros::this_node::getName() + "]::Velocity control is not enabled.");
}
target_joint_positions = simulated_joint_positions_;
}else{
target_joint_positions = robot_provider_->get_target_joint_positions();
}
}else {
target_joint_positions = current_joint_positions;
}
_update_vrep_position(target_joint_positions);
robot_provider_->send_joint_states(current_joint_positions, current_joint_velocity, VectorXd());
robot_provider_->send_joint_limits(joint_limits_);
@@ -119,7 +132,7 @@ int RobotDriverCoppelia::start_control_loop(){
void RobotDriverCoppelia::connect(){
auto ret = vi_->connect(configuration_.ip, configuration_.port, 100, 10);
auto ret = vi_->connect(configuration_.vrep_ip, configuration_.vrep_port, 100, 10);
if(!ret){
throw std::runtime_error("[RobotDriverCoppelia]::connect::Could not connect to Vrep");
}
@@ -133,19 +146,31 @@ void RobotDriverCoppelia::initialize(){
if(configuration_.using_real_robot)
{
ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize...");
ros::spinOnce();
int count = 0;
while (!real_robot_interface_->is_enabled()) {
ros::spinOnce();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
count++;
if(count > REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT){
ROS_ERROR_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized. Exiting on TIMEOUT...");
throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized.");
}
if(!ros::ok()) {
ROS_WARN_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved. Exiting...");
throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved, not OK.");
}
}
ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface initialized.");
joint_limits_ = real_robot_interface_->get_joint_limits();
_update_vrep_position(real_robot_interface_->get_joint_positions());
_update_vrep_position(real_robot_interface_->get_joint_positions(), true);
}else{
ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Simulation mode.");
// initialization information for robot driver provider
/**
* TODO: check for making sure real robot is not actually connected
*/
auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_robot_joint_names);
auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_joint_names);
VectorXd current_joint_velocity;
if(robot_mode_ == ControlMode::Velocity)
{current_joint_velocity = VectorXd::Zero(current_joint_positions.size());}
@@ -191,8 +216,8 @@ void RobotDriverCoppelia::_velocity_control_simulation_thread_main(){
current_joint_positions += tau * simulated_joint_velocities_; // no dynamic model
// cap joint limit
auto q_min = joint_limits_.first;
auto q_max = joint_limits_.second;
auto q_min = std::get<0>(joint_limits_);
auto q_max = std::get<1>(joint_limits_);
for (int i = 0; i < current_joint_positions.size(); i++) {
if (current_joint_positions(i) < q_min(i)) {
current_joint_positions(i) = q_min(i);

28
src/coppelia/sas_robot_driver_coppelia.h
View File

@@ -40,12 +40,17 @@
#include <dqrobotics/DQ.h>
#include <sas_robot_driver/sas_robot_driver.h>
#include <sas_clock/sas_clock.h>
#include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
// #include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
// #include <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
#include <thread>
#include <atomic>
#include <sas_robot_driver/sas_robot_driver_interface.h>
#include <sas_robot_driver/sas_robot_driver_provider.h>
#define VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC 2000000 // 2ms, 500Hz
#define REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT 500
using namespace DQ_robotics;
using namespace Eigen;
@@ -65,8 +70,9 @@ struct RobotDriverCoppeliaConfiguration
std::string robot_mode;
bool using_real_robot;
std::string robot_topic_prefix;
VectorXd q_min;
VectorXd q_max;
std::string robot_parameter_file_path;
// VectorXd q_min;
// VectorXd q_max;
};
class RobotDriverCoppelia
@@ -79,8 +85,8 @@ private:
RobotDriverCoppeliaConfiguration configuration_;
sas::Clock clock_;
atomic_bool* break_loops_;
Clock clock_;
std::atomic_bool* break_loops_;
bool _should_shutdown() const {return (*break_loops_);}
ControlMode robot_mode_;
@@ -97,20 +103,20 @@ private:
VectorXd simulated_joint_velocities_;
void start_simulation_thread(); // thread entry point
void _velocity_control_simulation_thread_main();
atomic_bool simulation_thread_started_ = false;
std::atomic_bool simulation_thread_started_{false};
bool initialized_ = false;
inline void _assert_initialized(){
inline void _assert_initialized() const{
if (!initialized_){throw std::runtime_error("[RobotDriverCoppelia] Robot driver not initialized");}
}
inline void _assert_is_alive(){
inline void _assert_is_alive() const{
if(!configuration_.using_real_robot && !simulation_thread_started_){throw std::runtime_error("[RobotDriverCoppelia] Robot Simulation is not alive");}
}
void _start_control_loop();
protected:
inline void _update_vrep_position(VectorXd joint_positions);
inline void _update_vrep_velocity(VectorXd joint_velocity);
inline void _update_vrep_position(const VectorXd &joint_positions, const bool& force_update=false) const;
inline void _update_vrep_velocity(const VectorXd & joint_velocity) const;
public:
RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;

13
src/sas_robot_driver_coppelia_node.cpp
View File

@@ -54,6 +54,7 @@ void sig_int_handler(int)
}
int main(int argc, char **argv)
{
if(signal(SIGINT, sig_int_handler) == SIG_ERR)
@@ -78,15 +79,19 @@ int main(int argc, char **argv)
sas::get_ros_param(nh,"/robot_mode", robot_driver_coppelia_configuration.robot_mode);
sas::get_ros_param(nh,"/using_real_robot", robot_driver_coppelia_configuration.using_real_robot);
sas::get_ros_param(nh,"/robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix);
sas::get_ros_param(nh,"/q_min", robot_driver_coppelia_configuration.q_min);
sas::get_ros_param(nh,"/q_max", robot_driver_coppelia_configuration.q_max);
sas::get_ros_param(nh,"/robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);
// std::vector<double> q_min_vec, q_max_vec;
// sas::get_ros_param(nh,"/q_min", q_min_vec);
// robot_driver_coppelia_configuration.q_min = sas::std_vector_double_to_vectorxd(q_min_vec);
// sas::get_ros_param(nh,"/q_max", q_max_vec);
// robot_driver_coppelia_configuration.q_max = sas::std_vector_double_to_vectorxd(q_max_vec);
robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
try
{
ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Coppelia robot.");
sas::RobotDriverCoppelia robot_driver_coppelia(robot_driver_coppelia_configuration,
sas::RobotDriverCoppelia robot_driver_coppelia(nh, robot_driver_coppelia_configuration,
&kill_this_process);
return robot_driver_coppelia.start_control_loop();