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

updated the coppelia driver

Browse Source
This commit is contained in:
Juancho
2023-09-13 17:49:19 +09:00
parent b8edcd86bb
commit 53e3ec6ae2
1 changed files with 11 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
src/sas_robot_driver_coppelia.cpp
View File

@@ -17,7 +17,7 @@ RobotDriver(break_loops),
vi_ = std::make_shared<DQ_VrepInterface>(); vi_ = std::make_shared<DQ_VrepInterface>();
desired_joint_velocities_ = VectorXd::Zero(dim_configuration_space_); desired_joint_velocities_ = VectorXd::Zero(dim_configuration_space_);
auto nodehandle = ros::NodeHandle(); auto nodehandle = ros::NodeHandle();
std::cout<<"Rostopic: "<<"/"+real_robot_topic_prefix_<<std::endl; std::cout<<"RobotDriverCoppelia::Rostopic: "<<"/"+real_robot_topic_prefix_<<std::endl;
franka1_ros_ = std::make_shared<sas::RobotDriverInterface>(nodehandle, "/"+real_robot_topic_prefix_); franka1_ros_ = std::make_shared<sas::RobotDriverInterface>(nodehandle, "/"+real_robot_topic_prefix_);
} }
@@ -53,7 +53,7 @@ void RobotDriverCoppelia::connect()
vi_->connect(configuration_.ip, configuration_.port, 500, 10); vi_->connect(configuration_.ip, configuration_.port, 500, 10);
vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_)); vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_));
std::cout<<"Connecting..."<<std::endl; std::cout<<"RobotDriverCoppelia::Connecting..."<<std::endl;
} }
void RobotDriverCoppelia::disconnect() void RobotDriverCoppelia::disconnect()
@@ -67,7 +67,7 @@ void RobotDriverCoppelia::disconnect()
{ {
joint_velocity_control_mirror_mode_thread_.join(); joint_velocity_control_mirror_mode_thread_.join();
} }
std::cout<<"Disconnected."<<std::endl; std::cout<<"RobotDriverCoppelia::Disconnected."<<std::endl;
} }
void RobotDriverCoppelia::initialize() void RobotDriverCoppelia::initialize()
@@ -79,7 +79,7 @@ void RobotDriverCoppelia::initialize()
}else{ }else{
_start_joint_velocity_control_mirror_thread(); _start_joint_velocity_control_mirror_thread();
} }
std::cout<<"Velocity loop running..."<<std::endl; std::cout<<"RobotDriverCoppelia::Velocity loop running..."<<std::endl;
} }
void RobotDriverCoppelia::deinitialize() void RobotDriverCoppelia::deinitialize()
@@ -87,7 +87,7 @@ void RobotDriverCoppelia::deinitialize()
vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_)); vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_));
vi_->stop_simulation(); vi_->stop_simulation();
finish_motion(); finish_motion();
std::cout<<"Deinitialized."<<std::endl; std::cout<<"RobotDriverCoppelia::Deinitialized."<<std::endl;
} }
@@ -146,11 +146,11 @@ void RobotDriverCoppelia::_start_joint_velocity_control_mode()
} }
catch(const std::exception& e) catch(const std::exception& e)
{ {
std::cout<<"sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " << e.what()<<std::endl; std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " << e.what()<<std::endl;
} }
catch(...) catch(...)
{ {
std::cout<<"sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " <<std::endl; std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " <<std::endl;
} }
@@ -189,7 +189,7 @@ void RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode()
try{ try{
finish_motion_ = false; finish_motion_ = false;
std::cout<<"Waiting for real robot topics..."<<std::endl; std::cout<<"RobotDriverCoppelia::Waiting for real robot topics..."<<std::endl;
VectorXd q; VectorXd q;
while (ros::ok()) { while (ros::ok()) {
if (franka1_ros_->is_enabled()) if (franka1_ros_->is_enabled())
@@ -199,7 +199,7 @@ void RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode()
} }
ros::spinOnce(); ros::spinOnce();
} }
std::cout<<"Done!"<<std::endl; std::cout<<"RobotDriverCoppelia::Done!"<<std::endl;
VectorXd q_vrep = vi_->get_joint_positions(jointnames_); VectorXd q_vrep = vi_->get_joint_positions(jointnames_);
VectorXd q_dot_vrep = vi_->get_joint_velocities(jointnames_); VectorXd q_dot_vrep = vi_->get_joint_velocities(jointnames_);
@@ -237,11 +237,11 @@ void RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode()
} }
catch(const std::exception& e) catch(const std::exception& e)
{ {
std::cout<<"sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " << e.what()<<std::endl; std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " << e.what()<<std::endl;
} }
catch(...) catch(...)
{ {
std::cout<<"sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " <<std::endl; std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " <<std::endl;
} }