Skip to content

Feature/rsi io #211

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
farshbafdoustar wants to merge 7 commits into
base: melodic-devel
Choose a base branch
from
Open

Feature/rsi io #211

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
b3ca42f
adding compatibility to RSI 4.x by providing a seperate ros_rsi.rsix
farshbafdoustar Jul 9, 2019
7eb206f
removing file extension inside RSI_CREATE. rsi 4.x add .rsix automati…
farshbafdoustar Jul 9, 2019
aba6885
making a hw interface plugin from kuka rsi
farshbafdoustar Dec 13, 2019
3568b21
chnaging to pos val interface
farshbafdoustar Jan 15, 2020
0e3d783
adding install to makefiles and adapting kuka interface to simulation…
farshbafdoustar Aug 6, 2021
e99e944
adding io to rsi
farshbafdoustar Dec 16, 2021
4c4ef13
rsi config change in krc4 for DO and DI
farshbafdoustar Dec 16, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions kuka_rsi_hw_interface/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
realtime_tools
roscpp
std_msgs
kuka_rsi_hw_interface_msgs
)

find_package(Boost REQUIRED COMPONENTS system)
Expand Down Expand Up @@ -58,6 +59,9 @@ target_link_libraries(kuka_hardware_interface_node
kuka_hardware_interface
)

install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})

install(
TARGETS kuka_hardware_interface
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
Expand All @@ -67,3 +71,10 @@ install(
install(
TARGETS kuka_hardware_interface_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)

install(FILES kuka_rsi_hardware_interface_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
40 changes: 28 additions & 12 deletions kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,14 +47,16 @@
// ROS
#include <ros/ros.h>
#include <std_msgs/String.h>

#include <pluginlib/class_list_macros.hpp>

// ros_control
#include <realtime_tools/realtime_publisher.h>
#include <controller_manager/controller_manager.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/posvel_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <hardware_state_command_interfaces/digital_io_command_interface.h>
#include <hardware_state_command_interfaces/digital_io_state_interface.h>

// Timers
#include <chrono>
Expand All @@ -66,23 +68,25 @@
#include <kuka_rsi_hw_interface/rsi_state.h>
#include <kuka_rsi_hw_interface/rsi_command.h>

#include <kuka_rsi_hw_interface_msgs/RSIReceived.h>
#include <kuka_rsi_hw_interface_msgs/RSISent.h>

namespace kuka_rsi_hw_interface
{

static const double RAD2DEG = 57.295779513082323;
static const double DEG2RAD = 0.017453292519943295;

class KukaHardwareInterface : public hardware_interface::RobotHW
{

private:

// ROS node handle
ros::NodeHandle nh_;

unsigned int n_dof_;

std::vector<std::string> joint_names_;
std::vector<std::map<std::string, std::string>> digital_outputs_;
std::vector<std::map<std::string, std::string>> digital_inputs_;

std::vector<double> joint_position_;
std::vector<double> joint_velocity_;
Expand All @@ -91,14 +95,21 @@ class KukaHardwareInterface : public hardware_interface::RobotHW
std::vector<double> joint_velocity_command_;
std::vector<double> joint_effort_command_;

std::vector<hardware_state_command_interfaces::DigitalIOStateHandle::State> digital_output_state_;
std::vector<hardware_state_command_interfaces::DigitalIOStateHandle::State> digital_output_command_;
std::vector<hardware_state_command_interfaces::DigitalIOStateHandle::State> digital_input_state_;

// RSI
RSIState rsi_state_;
RSICommand rsi_command_;
std::vector<double> rsi_initial_joint_positions_;
std::vector<double> rsi_joint_position_corrections_;
std::vector<int> rsi_digital_outputs;

unsigned long long ipoc_;

std::unique_ptr<realtime_tools::RealtimePublisher<std_msgs::String> > rt_rsi_pub_;
std::unique_ptr<realtime_tools::RealtimePublisher<kuka_rsi_hw_interface_msgs::RSIReceived>> rt_rsi_kuka_to_pc_pub_;
std::unique_ptr<realtime_tools::RealtimePublisher<kuka_rsi_hw_interface_msgs::RSISent>> rt_rsi_pc_to_kuka_pub_;

std::unique_ptr<UDPServer> server_;
std::string local_host_;
Expand All @@ -115,20 +126,25 @@ class KukaHardwareInterface : public hardware_interface::RobotHW

// Interfaces
hardware_interface::JointStateInterface joint_state_interface_;
hardware_interface::PositionJointInterface position_joint_interface_;
// hardware_interface::PositionJointInterface position_joint_interface_;
hardware_interface::PosVelJointInterface position_velocity_joint_interface_;
hardware_state_command_interfaces::DigitalInputStateInterface digital_input_state_interface_;
hardware_state_command_interfaces::DigitalOutputStateInterface digital_output_state_interface_;
hardware_state_command_interfaces::DigitalOutputCommandInterface digital_output_command_interface_;
bool first_time_ = true;

public:

KukaHardwareInterface();
~KukaHardwareInterface();

void start();
void configure();
bool read(const ros::Time time, const ros::Duration period);
bool write(const ros::Time time, const ros::Duration period);

bool init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw_nh);
void read(const ros::Time& time, const ros::Duration& period);
void write(const ros::Time& time, const ros::Duration& period);
bool read();
};

} // namespace kuka_rsi_hw_interface
} // namespace kuka_rsi_hw_interface

#endif
34 changes: 29 additions & 5 deletions kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_command.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,21 +44,25 @@

namespace kuka_rsi_hw_interface
{

class RSICommand
{
public:
RSICommand();
RSICommand(std::vector<double> position_corrections, unsigned long long ipoc);
RSICommand(std::vector<double> position_corrections, std::vector<int> digital_outputs, unsigned long long ipoc);
std::string xml_doc;
};

RSICommand::RSICommand()
{
// Intentionally empty
}

RSICommand::RSICommand(std::vector<double> joint_position_correction, unsigned long long ipoc)
: RSICommand(joint_position_correction,
std::initializer_list<int>({ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }), ipoc)
{
}
RSICommand::RSICommand(std::vector<double> joint_position_correction, std::vector<int> digital_outputs,
unsigned long long ipoc)
{
TiXmlDocument doc;
TiXmlElement* root = new TiXmlElement("Sen");
Expand All @@ -71,8 +75,28 @@ RSICommand::RSICommand(std::vector<double> joint_position_correction, unsigned l
el->SetAttribute("A4", std::to_string(joint_position_correction[3]));
el->SetAttribute("A5", std::to_string(joint_position_correction[4]));
el->SetAttribute("A6", std::to_string(joint_position_correction[5]));

root->LinkEndChild(el);

TiXmlElement* out = new TiXmlElement("Out");
out->SetAttribute("Ch01", std::to_string(digital_outputs[0]));
out->SetAttribute("Ch02", std::to_string(digital_outputs[1]));
out->SetAttribute("Ch03", std::to_string(digital_outputs[2]));
out->SetAttribute("Ch04", std::to_string(digital_outputs[3]));
out->SetAttribute("Ch05", std::to_string(digital_outputs[4]));
out->SetAttribute("Ch06", std::to_string(digital_outputs[5]));
out->SetAttribute("Ch07", std::to_string(digital_outputs[6]));
out->SetAttribute("Ch08", std::to_string(digital_outputs[7]));
out->SetAttribute("Ch09", std::to_string(digital_outputs[8]));
out->SetAttribute("Ch10", std::to_string(digital_outputs[9]));
out->SetAttribute("Ch11", std::to_string(digital_outputs[10]));
out->SetAttribute("Ch12", std::to_string(digital_outputs[11]));
out->SetAttribute("Ch13", std::to_string(digital_outputs[12]));
out->SetAttribute("Ch14", std::to_string(digital_outputs[13]));
out->SetAttribute("Ch15", std::to_string(digital_outputs[14]));
out->SetAttribute("Ch16", std::to_string(digital_outputs[15]));

// root->LinkEndChild(out);

el = new TiXmlElement("IPOC");
el->LinkEndChild(new TiXmlText(std::to_string(ipoc)));
root->LinkEndChild(el);
Expand All @@ -84,6 +108,6 @@ RSICommand::RSICommand(std::vector<double> joint_position_correction, unsigned l
xml_doc = printer.Str();
}

} // namespace kuka_rsi_hw_interface
} // namespace kuka_rsi_hw_interface

#endif
74 changes: 58 additions & 16 deletions kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/rsi_state.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@

/*
* Author: Lars Tingelstad <[email protected]>
*/
*/

#ifndef KUKA_RSI_HW_INTERFACE_RSI_STATE_
#define KUKA_RSI_HW_INTERFACE_RSI_STATE_
Expand All @@ -44,19 +44,18 @@

namespace kuka_rsi_hw_interface
{

class RSIState
{

private:
std::string xml_doc_;

public:
RSIState() :
positions(6, 0.0),
initial_positions(6, 0.0),
cart_position(6, 0.0),
initial_cart_position(6, 0.0)
RSIState()
: positions(6, 0.0)
, initial_positions(6, 0.0)
, cart_position(6, 0.0)
, initial_cart_position(6, 0.0)
, digital_inputs(16, 0)
{
xml_doc_.resize(1024);
}
Expand All @@ -70,17 +69,20 @@ class RSIState
std::vector<double> cart_position;
// RSol
std::vector<double> initial_cart_position;
// Digital Inputs ; kuka sends it as 0 or 1
std::vector<int> digital_inputs;
// IPOC
unsigned long long ipoc;

bool incompatible_xml = false;
};

RSIState::RSIState(std::string xml_doc) :
xml_doc_(xml_doc),
positions(6, 0.0),
initial_positions(6, 0.0),
cart_position(6, 0.0),
initial_cart_position(6, 0.0)
RSIState::RSIState(std::string xml_doc)
: xml_doc_(xml_doc)
, positions(6, 0.0)
, initial_positions(6, 0.0)
, cart_position(6, 0.0)
, initial_cart_position(6, 0.0)
, digital_inputs(16, 0)
{
// Parse message from robot
TiXmlDocument bufferdoc;
Expand Down Expand Up @@ -119,11 +121,51 @@ RSIState::RSIState(std::string xml_doc) :
RSol_el->Attribute("A", &initial_cart_position[3]);
RSol_el->Attribute("B", &initial_cart_position[4]);
RSol_el->Attribute("C", &initial_cart_position[5]);
// Extract Digital Inputs
// They should not start with number other wise the parser will have erro
// so the buffer should not be like <In 01=".."/>
// It should be lilke <In Ch01=".."/>
TiXmlElement* In_el = rob->FirstChildElement("In");
if (In_el)
{
bool success = true;

success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch01", &digital_inputs[0]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch02", &digital_inputs[1]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch03", &digital_inputs[2]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch04", &digital_inputs[3]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch05", &digital_inputs[4]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch06", &digital_inputs[5]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch07", &digital_inputs[6]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch08", &digital_inputs[7]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch09", &digital_inputs[8]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch10", &digital_inputs[9]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch11", &digital_inputs[10]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch12", &digital_inputs[11]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch13", &digital_inputs[12]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch14", &digital_inputs[13]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch15", &digital_inputs[14]);
success &= TIXML_SUCCESS == In_el->QueryIntAttribute("Ch16", &digital_inputs[15]);
// if (!success && !incompatible_xml)
// {
// std::cout << "Recieved RSI XML does not have 16 Inputs" << std::endl;
// incompatible_xml = true;
// }
}
else
{
// if (!incompatible_xml)
// {
// std::cout << "Recieved RSI XML does not have In[xx]" << std::endl;
// incompatible_xml = true;
// }
}

// Get the IPOC timestamp
TiXmlElement* ipoc_el = rob->FirstChildElement("IPOC");
ipoc = std::stoull(ipoc_el->FirstChild()->Value());
}

} // namespace kuka_rsi_hw_interface
} // namespace kuka_rsi_hw_interface

#endif
Loading