From 97add752a168209d96ee5b42da60618bf25aee22 Mon Sep 17 00:00:00 2001 From: Simon Rasmussen Date: Thu, 13 Apr 2017 10:47:41 +0200 Subject: [PATCH] Implemented ros control --- .../ur_modern_driver/ros/hardware_interface.h | 168 ++++++++++++++++++ include/ur_modern_driver/ros/io_service.h | 61 +++++++ include/ur_modern_driver/ros/mb_publisher.h | 55 ++++++ include/ur_modern_driver/ros/robot_hardware.h | 46 +++++ include/ur_modern_driver/ros/ros_controller.h | 60 +++++++ src/ros/mb_publisher.cpp | 54 ++++++ src/ros/robot_hardware.cpp | 76 ++++++++ 7 files changed, 520 insertions(+) create mode 100644 include/ur_modern_driver/ros/hardware_interface.h create mode 100644 include/ur_modern_driver/ros/io_service.h create mode 100644 include/ur_modern_driver/ros/mb_publisher.h create mode 100644 include/ur_modern_driver/ros/robot_hardware.h create mode 100644 include/ur_modern_driver/ros/ros_controller.h create mode 100644 src/ros/mb_publisher.cpp create mode 100644 src/ros/robot_hardware.cpp diff --git a/include/ur_modern_driver/ros/hardware_interface.h b/include/ur_modern_driver/ros/hardware_interface.h new file mode 100644 index 0000000..e970e30 --- /dev/null +++ b/include/ur_modern_driver/ros/hardware_interface.h @@ -0,0 +1,168 @@ +#pragma once +#include +#include +#include +#include +#include +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ur/rt_state.h" + +class HardwareInterface +{ +public: + virtual void write() = 0; + virtual void start() + { + } + virtual void stop() + { + } +}; + +using hardware_interface::JointHandle; +using hardware_interface::JointStateHandle; +using hardware_interface::JointStateInterface; + +class JointInterface : public JointStateInterface +{ +private: + std::array velocities_, positions_, efforts_; + +public: + JointInterface(std::vector& joint_names) + { + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointStateHandle(joint_names[i], &positions_[i], &velocities_[i], &efforts_[i])); + } + } + + void update(RTShared& packet) + { + positions_ = packet.q_actual; + velocities_ = packet.qd_actual; + efforts_ = packet.i_actual; + } +}; + +class WrenchInterface : public hardware_interface::ForceTorqueSensorInterface +{ + std::array tcp_; +public: + WrenchInterface() + { + registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin()+3)); + } + + void update(RTShared& packet) + { + tcp_ = packet.tcp_force; + } +}; + +class VelocityInterface : public HardwareInterface, public hardware_interface::VelocityJointInterface +{ +private: + URCommander& commander_; + std::array velocity_cmd_, prev_velocity_cmd_; + double max_vel_change_; + +public: + VelocityInterface(URCommander& commander, JointStateInterface& js_interface, std::vector& joint_names) + : commander_(commander) + { + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &velocity_cmd_[i])); + } + } + + virtual void write() + { + for (size_t i = 0; i < 6; i++) + { + double prev = prev_velocity_cmd_[i]; + double lo = prev - max_vel_change_; + double hi = prev + max_vel_change_; + // clamp value to ±max_vel_change + prev_velocity_cmd_[i] = std::max(lo, std::min(velocity_cmd_[i], hi)); + } + + //times 125??? + commander_.speedj(prev_velocity_cmd_, max_vel_change_ * 125); + } +}; + +static const std::string POSITION_PROGRAM = R"( +def driverProg(): + MULT_jointstate = XXXXX + + SERVO_IDLE = 0 + SERVO_RUNNING = 1 + cmd_servo_state = SERVO_IDLE + cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + def set_servo_setpoint(q): + enter_critical + cmd_servo_state = SERVO_RUNNING + cmd_servo_q = q + exit_critical + end + thread servoThread(): + state = SERVO_IDLE + while True: + enter_critical + q = cmd_servo_q + do_brake = False + if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE): + do_brake = True + end + state = cmd_servo_state + cmd_servo_state = SERVO_IDLE + exit_critical + if do_brake: + stopj(1.0) + sync() + elif state == SERVO_RUNNING: + servoj(q, YYYYYYYY) + else: + sync() + end + end + end + + thread_servo = run servoThread() + keepalive = 1 + while keepalive > 0: + params_mult = socket_read_binary_integer(6+1) + if params_mult[0] > 0: + q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] + keepalive = params_mult[7] + set_servo_setpoint(q) + end + end + sleep(.1) + socket_close() + kill thread_servo +end +)"; + +class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface +{ +private: + URCommander& commander_; + std::array position_cmd_; + +public: + PositionInterface(URCommander& commander, JointStateInterface& js_interface, std::vector& joint_names) + : commander_(commander) + { + for (size_t i = 0; i < 6; i++) + { + registerHandle(JointHandle(js_interface.getHandle(joint_names[i]), &position_cmd_[i])); + } + } + + virtual void write() + { + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/io_service.h b/include/ur_modern_driver/ros/io_service.h new file mode 100644 index 0000000..be59376 --- /dev/null +++ b/include/ur_modern_driver/ros/io_service.h @@ -0,0 +1,61 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/commander.h" + +class IOService +{ +private: + ros::NodeHandle nh_; + URCommander &commander_; + ros::ServiceServer io_service_; + ros::ServiceServer payload_service_; + + bool setIO(ur_msgs::SetIORequest& req, ur_msgs::SetIOResponse& resp) + { + LOG_INFO("setIO called with [%d, %d]", req.fun, req.pin); + bool res = false; + bool flag = req.state > 0.0 ? true : false; + switch(req.fun) + { + case ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT: + res = commander_.setDigitalOut(req.pin, flag); + break; + case ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT: + res = commander_.setAnalogOut(req.pin, req.state); + break; + case ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE: + res = commander_.setToolVoltage(static_cast(req.state)); + break; + case ur_msgs::SetIO::Request::FUN_SET_FLAG: + res = commander_.setFlag(flag); + break; + } + + return resp.success = res; + } + + bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp) + { + LOG_INFO("setPayload called"); + //TODO check min and max payload? + return resp.success = commander_.setPayload(req.payload); + } + + +public: + IOService(URCommander &commander) + : commander_(commander) + , io_service_(nh_.advertiseService("ur_driver/set_io", &IOService::setIO, this)) + , payload_service_(nh_.advertiseService("ur_driver/set_payload", &IOService::setPayload, this)) + { + + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/mb_publisher.h b/include/ur_modern_driver/ros/mb_publisher.h new file mode 100644 index 0000000..11c4d8b --- /dev/null +++ b/include/ur_modern_driver/ros/mb_publisher.h @@ -0,0 +1,55 @@ +#pragma once + +#include +#include +#include +#include + +#include "ur_modern_driver/ur/consumer.h" + +using namespace ros; + +class MBPublisher : public URStatePacketConsumer +{ +private: + NodeHandle nh_; + Publisher io_pub_; + + template + inline void appendDigital(std::vector &vec, std::bitset bits) + { + for(size_t i = 0; i < N; i++) + { + ur_msgs::Digital digi; + digi.pin = static_cast(i); + digi.state = bits.test(i); + vec.push_back(digi); + } + } + + void publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data); + +public: + MBPublisher() + : io_pub_(nh_.advertise("ur_driver/io_states", 1)) + { + } + + virtual bool consume(MasterBoardData_V1_X& data); + virtual bool consume(MasterBoardData_V3_0__1& data); + virtual bool consume(MasterBoardData_V3_2& data); + + virtual bool consume(RobotModeData_V1_X& data); + virtual bool consume(RobotModeData_V3_0__1& data); + virtual bool consume(RobotModeData_V3_2& data); + + virtual void setupConsumer() + { + } + virtual void teardownConsumer() + { + } + virtual void stopConsumer() + { + } +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/robot_hardware.h b/include/ur_modern_driver/ros/robot_hardware.h new file mode 100644 index 0000000..9664432 --- /dev/null +++ b/include/ur_modern_driver/ros/robot_hardware.h @@ -0,0 +1,46 @@ +#pragma once +#include +#include +#include +#include +#include +#include "ur_modern_driver/ros/hardware_interface.h" + +using hardware_interface::RobotHW; +using hardware_interface::ControllerInfo; + +class RobotHardware : public RobotHW +{ +private: + JointInterface joint_interface_; + WrenchInterface wrench_interface_; + PositionInterface position_interface_; + VelocityInterface velocity_interface_; + hardware_interface::ForceTorqueSensorInterface force_torque_interface_; + HardwareInterface* active_interface_; + std::vector available_interfaces_; + +public: + RobotHardware(URCommander& commander, std::vector& joint_names) + : joint_interface_(joint_names) + , wrench_interface_() + , position_interface_(commander, joint_interface_, joint_names) + , velocity_interface_(commander, joint_interface_, joint_names) + , available_interfaces_{&position_interface_, &velocity_interface_} + { + registerInterface(&joint_interface_); + registerInterface(&wrench_interface_); + registerInterface(&position_interface_); + registerInterface(&velocity_interface_); + } + + //bool canSwitch(const std::list& start_list, const std::list& stop_list) const; + void doSwitch(const std::list& start_list, const std::list& stop_list); + + + /// \brief Read the state from the robot hardware. + virtual void read(RTShared& packet); + + /// \brief write the command to the robot hardware. + virtual void write(); +}; \ No newline at end of file diff --git a/include/ur_modern_driver/ros/ros_controller.h b/include/ur_modern_driver/ros/ros_controller.h new file mode 100644 index 0000000..ad9f2ae --- /dev/null +++ b/include/ur_modern_driver/ros/ros_controller.h @@ -0,0 +1,60 @@ +#pragma once +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/pipeline.h" +#include "ur_modern_driver/ur/rt_state.h" +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ros/robot_hardware.h" + +class ROSController : public URRTPacketConsumer +{ +private: + ros::NodeHandle nh_; + ros::Time lastUpdate_; + RobotHardware robot_; + controller_manager::ControllerManager controller_; + +public: + ROSController(URCommander& commander, std::vector& joint_names) + : robot_(commander, joint_names) + , controller_(&robot_, nh_) + { + } + + virtual void setupConsumer() + { + lastUpdate_ = ros::Time::now(); + } + + bool handle(RTShared& state) + { + auto time = ros::Time::now(); + auto diff = time - lastUpdate_; + lastUpdate_ = time; + + robot_.read(state); + controller_.update(time, diff); + robot_.write(); + //todo: return result of write + return true; + } + + + virtual bool consume(RTState_V1_6__7& state) + { + return handle(state); + } + virtual bool consume(RTState_V1_8& state) + { + return handle(state); + } + virtual bool consume(RTState_V3_0__1& state) + { + return handle(state); + } + virtual bool consume(RTState_V3_2__3& state) + { + return handle(state); + } +}; \ No newline at end of file diff --git a/src/ros/mb_publisher.cpp b/src/ros/mb_publisher.cpp new file mode 100644 index 0000000..1177be5 --- /dev/null +++ b/src/ros/mb_publisher.cpp @@ -0,0 +1,54 @@ +#include "ur_modern_driver/ros/mb_publisher.h" + +inline void appendAnalog(std::vector &vec, double val, uint8_t pin) +{ + ur_msgs::Analog ana; + ana.pin = pin; + ana.state = val; + vec.push_back(ana); +} + +void MBPublisher::publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data) +{ + appendAnalog(io_msg.analog_in_states, data.analog_input0, 0); + appendAnalog(io_msg.analog_in_states, data.analog_input1, 1); + appendAnalog(io_msg.analog_out_states, data.analog_output0, 0); + appendAnalog(io_msg.analog_out_states, data.analog_output1, 1); + + io_pub_.publish(io_msg); +} + +bool MBPublisher::consume(MasterBoardData_V1_X& data) +{ + ur_msgs::IOStates io_msg; + appendDigital(io_msg.digital_in_states, data.digital_input_bits); + appendDigital(io_msg.digital_out_states, data.digital_output_bits); + publish(io_msg, data); + return true; +} +bool MBPublisher::consume(MasterBoardData_V3_0__1& data) +{ + ur_msgs::IOStates io_msg; + appendDigital(io_msg.digital_in_states, data.digital_input_bits); + appendDigital(io_msg.digital_out_states, data.digital_output_bits); + publish(io_msg, data); + return true; +} +bool MBPublisher::consume(MasterBoardData_V3_2& data) +{ + consume(static_cast(data)); + return true; +} + +bool MBPublisher::consume(RobotModeData_V1_X& data) +{ + return true; +} +bool MBPublisher::consume(RobotModeData_V3_0__1& data) +{ + return true; +} +bool MBPublisher::consume(RobotModeData_V3_2& data) +{ + return true; +} \ No newline at end of file diff --git a/src/ros/robot_hardware.cpp b/src/ros/robot_hardware.cpp new file mode 100644 index 0000000..cddcaff --- /dev/null +++ b/src/ros/robot_hardware.cpp @@ -0,0 +1,76 @@ +#include "ur_modern_driver/ros/robot_hardware.h" + +/* +bool RobotHardware::canSwitch(const std::list& start_list, + const std::list& stop_list) const +{ + + bool running = active_interface_ != nullptr; + size_t start_size = start_list.size(); + size_t stop_size = stop_list.size(); + + + for (auto const& ci : stop_list) + { + auto it = interfaces_.find(ci.hardware_interface); + if(it == interfaces_.end() || it->second != active_interface_) + return false; + } + + for (auto const& ci : start_list) + { + auto it = interfaces_.find(ci.hardware_interface); + //we can only start a controller that's already running if we stop it first + if(it == interfaces_.end() || (it->second == active_interface_ && stop_size == 0)) + return false; + } + + return true; +} +*/ + +void RobotHardware::doSwitch(const std::list& start_list, + const std::list& stop_list) +{ + if(active_interface_ != nullptr && stop_list.size() > 0) + { + active_interface_->stop(); + active_interface_ = nullptr; + } + + for(auto const& ci : start_list) + { + auto it = interfaces_.find(ci.hardware_interface); + if(it == interfaces_.end()) + continue; + + //we can only switch to one of the available interfaces + if(std::find(available_interfaces_.begin(), available_interfaces_.end(), it->second) == available_interfaces_.end()) + continue; + + auto new_interface = static_cast(it->second); + + if(new_interface == nullptr) + continue; + + LOG_INFO("Starting %s", ci.hardware_interface.c_str()); + active_interface_ = new_interface; + new_interface->start(); + + return; + } +} + +void RobotHardware::write() +{ + if(active_interface_ == nullptr) + return; + + active_interface_->write(); +} + +void RobotHardware::read(RTShared& packet) +{ + joint_interface_.update(packet); + wrench_interface_.update(packet); +} \ No newline at end of file