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

Added a new class to test the vrep communication in robots commanded by joints velocities

Browse Source
This commit is contained in:
Juancho
2023-08-03 11:53:57 +09:00
parent e45a160fd0
commit 51910be06d
3 changed files with 66 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
include/robot_coppelia_ros_interface.h
View File

@@ -21,7 +21,9 @@ struct RobotCoppeliaROSConfiguration
int thread_sampling_time_nsec;
int port;
std::string ip;
std::vector<std::string> jointnames;
std::string robot_mode;
bool mirror_mode;
};
class RobotCoppeliaRosInterface
@@ -31,13 +33,18 @@ private:
bool _should_shutdown() const;
sas::Clock clock_;
RobotCoppeliaROSConfiguration configuration_;
std::string robot_mode_ = std::string("VelocityControl"); // PositionControl
bool mirror_mode_ = false;
double gain_ = 0.5;
protected:
ros::NodeHandle nh_;
VectorXd joint_positions_;
VectorXd target_joint_positions_;
VectorXd target_joint_velocities_;
std::string topic_prefix_;
ros::Publisher publisher_target_joint_positions_;
//ros::Publisher publisher_target_joint_positions_;
ros::Publisher publisher_joint_states_;
//ros::Subscriber subscriber_joint_state_;
ros::Subscriber subscriber_target_joint_positions_;
@@ -47,6 +54,7 @@ protected:
std::shared_ptr<DQ_VrepInterface> vi_;
void _joint_states_callback(const sensor_msgs::JointState::ConstPtr& jointstate);
void _callback_target_joint_positions(const std_msgs::Float64MultiArrayConstPtr &msg);
void _callback_target_joint_velocities(const std_msgs::Float64MultiArrayConstPtr& msg);
void send_joint_states(const VectorXd &joint_positions, const VectorXd &joint_velocities, const VectorXd &joint_forces);
@@ -54,15 +62,15 @@ protected:
public:
RobotCoppeliaRosInterface(const ros::NodeHandle& nh,
const std::string& topic_prefix,
const std::vector<std::string>& jointnames,
const RobotCoppeliaROSConfiguration& configuration,
std::atomic_bool* kill_this_node);
RobotCoppeliaRosInterface()=delete;
~RobotCoppeliaRosInterface();
VectorXd get_joint_positions() const;
void set_joint_target_positions(const VectorXd& target_joint_positions);
void set_joint_target_velocities(const VectorXd& target_joint_velocities);
//VectorXd get_joint_positions() const;
//void set_joint_target_positions(const VectorXd& target_joint_positions);
//void set_joint_target_positions_(const VectorXd& target_joint_positions);
int control_loop();
};

70
src/robot_coppelia_ros_interface.cpp
View File

@@ -10,29 +10,26 @@
*/
RobotCoppeliaRosInterface::RobotCoppeliaRosInterface(const ros::NodeHandle& nh,
const std::string& topic_prefix,
const std::vector<std::string>& jointnames,
const RobotCoppeliaROSConfiguration& configuration,
std::atomic_bool* kill_this_node)
:nh_(nh),
topic_prefix_(topic_prefix),
jointnames_(jointnames),
kill_this_node_(kill_this_node),
configuration_(configuration),
robot_mode_ (configuration.robot_mode),
jointnames_(configuration.jointnames),
mirror_mode_(configuration.mirror_mode),
clock_(configuration.thread_sampling_time_nsec)
{
//subscriber_joint_state_ = nh_.subscribe(topic_prefix_+ "/get/joint_states", 1,
// &RobotCoppeliaRosInterface::_joint_states_callback, this);
//publisher_target_joint_positions_ = nh_.advertise<std_msgs::Float64MultiArray>
// (topic_prefix_ + "/set/target_joint_positions", 1);
subscriber_target_joint_positions_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_positions", 1, &RobotCoppeliaRosInterface::_callback_target_joint_positions, this);
subscriber_target_joint_positions_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_positions", 1, &RobotCoppeliaRosInterface::_callback_target_joint_positions, this);
subscriber_target_joint_velocities_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_velocities", 1, &RobotCoppeliaRosInterface::_callback_target_joint_velocities, this);
publisher_joint_states_ = nh_.advertise<sensor_msgs::JointState>(topic_prefix_+ "/get/joint_states", 1);
ROS_INFO_STREAM(ros::this_node::getName() << "::Connecting with CoppeliaSim...");
vi_ = std::make_shared<DQ_VrepInterface>();
vi_->connect(configuration_.ip, configuration_.port, 500, 10);
vi_->start_simulation();
ROS_INFO_STREAM(ros::this_node::getName() << "::Connection ok!");
}
RobotCoppeliaRosInterface::~RobotCoppeliaRosInterface()
@@ -40,20 +37,14 @@ RobotCoppeliaRosInterface::~RobotCoppeliaRosInterface()
vi_->disconnect();
}
/**
* @brief get_joint_positions
* @return
*/
/*
VectorXd RobotCoppeliaRosInterface::get_joint_positions() const
{
return joint_positions_;
}
/**
* @brief set_joint_target_positions
* @param target_joint_positions
*/
/*
void RobotCoppeliaRosInterface::set_joint_target_positions(const VectorXd &target_joint_positions)
{
@@ -62,12 +53,7 @@ void RobotCoppeliaRosInterface::set_joint_target_positions(const VectorXd &targe
publisher_target_joint_positions_.publish(ros_msg);
}
void RobotCoppeliaRosInterface::set_joint_target_velocities(const VectorXd &target_joint_velocities)
{
}
*/
void RobotCoppeliaRosInterface::send_joint_states(const VectorXd &joint_positions, const VectorXd &joint_velocities, const VectorXd &joint_forces)
{
@@ -85,7 +71,7 @@ int RobotCoppeliaRosInterface::control_loop()
{
try{
clock_.init();
ROS_INFO_STREAM(ros::this_node::getName() << "::Waiting to connect with CoppeliaSim...");
ROS_INFO_STREAM(ros::this_node::getName() << "::Starting control loop...");
while(not _should_shutdown())
@@ -97,16 +83,29 @@ int RobotCoppeliaRosInterface::control_loop()
joint_positions_ = q;
send_joint_states(q, VectorXd(), VectorXd());
if (target_joint_positions_.size()>0)
if (robot_mode_ == std::string("VelocityControl"))
{
vi_->set_joint_target_positions(jointnames_, target_joint_positions_);
//if (mirror_mode_ == true)
//{
if (target_joint_positions_.size()>0)
{
vi_->set_joint_target_velocities(jointnames_, gain_*(target_joint_positions_-q));
}
//}
//else{
if (target_joint_velocities_.size()>0)
{
vi_->set_joint_target_velocities(jointnames_, target_joint_velocities_);
}
//}
}
//auto q = get_joint_positions();
std::cout<<"q: "<<q.transpose()<<std::endl;
std::cout<<"target q: "<<target_joint_positions_.transpose()<<std::endl;
else if (robot_mode_ == std::string("PositionControl"))
{
if (target_joint_positions_.size()>0)
{
vi_->set_joint_target_positions(jointnames_, target_joint_positions_);
}
}
ros::spinOnce();
}
@@ -143,3 +142,8 @@ void RobotCoppeliaRosInterface::_callback_target_joint_positions(const std_msgs:
{
target_joint_positions_ = sas::std_vector_double_to_vectorxd(msg->data);
}
void RobotCoppeliaRosInterface::_callback_target_joint_velocities(const std_msgs::Float64MultiArrayConstPtr &msg)
{
target_joint_velocities_ = sas::std_vector_double_to_vectorxd(msg->data);
}

16
src/robot_coppelia_ros_interface_node.cpp
View File

@@ -84,12 +84,26 @@ int main(int argc, char **argv)
//4000000
RobotCoppeliaROSConfiguration configuration;
sas::get_ros_param(nh,"/vrep_ip", configuration.ip);
sas::get_ros_param(nh,"/robot_mode", configuration.robot_mode);
sas::get_ros_param(nh,"/thread_sampling_time_nsec",configuration.thread_sampling_time_nsec);
sas::get_ros_param(nh,"/vrep_port", configuration.port);
sas::get_ros_param(nh,"/vrep_robot_joint_names", configuration.jointnames);
sas::get_ros_param(nh,"/mirror_mode", configuration.mirror_mode);
/*
configuration.thread_sampling_time_nsec = 4000000;
configuration.ip = "10.198.113.159";
configuration.port = 20021;
std::vector<std::string> jointnames = {"Franka_joint1","Franka_joint2","Franka_joint3",
"Franka_joint4","Franka_joint5","Franka_joint6","Franka_joint7"};
RobotCoppeliaRosInterface robot_coppelia_ros_interface(nh, "/franka_1_sim",jointnames, configuration, &kill_this_process);
configuration.robot_mode = std::string("VelocityControl");
configuration.mirror_mode = true;
configuration.jointnames = jointnames;
*/
RobotCoppeliaRosInterface robot_coppelia_ros_interface(nh, "/franka_1_sim", configuration, &kill_this_process);
try
{