diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h index 34edb85..5d061c8 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h @@ -43,8 +43,9 @@ public: * \brief Constructs a new UrDriver object. * * \param robot_ip IP-address under which the robot is reachable. + * \param script_file URScript file that should be sent to the robot */ - UrDriver(const std::string& robot_ip); + UrDriver(const std::string& robot_ip, const std::string& script_file); virtual ~UrDriver() = default; /*! @@ -64,6 +65,8 @@ public: bool writeJointCommand(const vector6d_t& values); private: + std::string readScriptFile(const std::string& filename); + int rtde_frequency_; comm::INotifier notifier_; std::unique_ptr rtde_client_; diff --git a/ur_rtde_driver/launch/ur10_ros_control.launch b/ur_rtde_driver/launch/ur10_ros_control.launch index 445d971..8fac166 100644 --- a/ur_rtde_driver/launch/ur10_ros_control.launch +++ b/ur_rtde_driver/launch/ur10_ros_control.launch @@ -18,6 +18,7 @@ + @@ -25,8 +26,9 @@ - + + diff --git a/ur_rtde_driver/resources/servoj.urscript b/ur_rtde_driver/resources/servoj.urscript new file mode 100644 index 0000000..02be93d --- /dev/null +++ b/ur_rtde_driver/resources/servoj.urscript @@ -0,0 +1,76 @@ +def myProg(): + global steptime = get_steptime() + textmsg("steptime=", steptime) + MULT_jointstate = {{JOINT_STATE_REPLACE}} + + SERVO_UNINITIALIZED = -1 + SERVO_IDLE = 0 + SERVO_RUNNING = 1 + cmd_servo_state = SERVO_UNINITIALIZED + cmd_servo_q = get_actual_joint_positions() + cmd_servo_q_last = get_actual_joint_positions() + def set_servo_setpoint(q): + enter_critical + cmd_servo_state = SERVO_RUNNING + cmd_servo_q_last = cmd_servo_q + cmd_servo_q = q + exit_critical + end + + def extrapolate(): + enter_critical + diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]] + cmd_servo_q_last = cmd_servo_q + cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]] + exit_critical + + return cmd_servo_q + end + + thread servoThread(): + state = SERVO_IDLE + while True: + enter_critical + q = cmd_servo_q + do_extrapolate = False + if (cmd_servo_state == SERVO_IDLE): + do_extrapolate = True + end + state = cmd_servo_state + cmd_servo_state = SERVO_IDLE + exit_critical + if do_extrapolate: + textmsg("No new setpoint received. Extrapolating.") + q = extrapolate() + servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + elif state == SERVO_RUNNING: + servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + else: + textmsg("Should not be here") + sync() + end + end + stopj(0.1) + end + socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") + + thread_servo = run servoThread() + keepalive = -2 + params_mult = socket_read_binary_integer(6+1, "reverse_socket") + keepalive = params_mult[7] + while keepalive > 0: + params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.2) # steptime could work as well, but does not work in simulation + if params_mult[0] > 0: + keepalive = params_mult[7] + 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] + set_servo_setpoint(q) + else: + # TODO: Extrapolation goes here + keepalive = keepalive - 1 + end + end + sleep(.1) + socket_close() + kill thread_servo +end + diff --git a/ur_rtde_driver/src/main_plain_driver.cpp b/ur_rtde_driver/src/main_plain_driver.cpp index 005283c..44eb4a2 100644 --- a/ur_rtde_driver/src/main_plain_driver.cpp +++ b/ur_rtde_driver/src/main_plain_driver.cpp @@ -18,14 +18,15 @@ using namespace ur_driver; int main(int argc, char* argv[]) { - std::string ROBOT_IP = "192.168.56.101"; + std::string robot_ip = "192.168.56.101"; + std::string script_filename = "urprog.urscript"; - if (argc > 1) + if (argc > 2) { - ROBOT_IP = argv[1]; + robot_ip = argv[1]; } - UrDriver driver(ROBOT_IP); + UrDriver driver(robot_ip, script_filename); while (true) { diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 397687a..00839dc 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -45,9 +45,15 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h joint_velocities_ = { { 0, 0, 0, 0, 0, 0 } }; joint_efforts_ = { { 0, 0, 0, 0, 0, 0 } }; std::string robot_ip = robot_hw_nh.param("robot_ip", "192.168.56.101"); + std::string script_filename; + if (!robot_hw_nh.getParam("script_file", script_filename)) + { + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("script_file") << " not given."); + return false; + } ROS_INFO_STREAM("Initializing urdriver"); - ur_driver_.reset(new UrDriver(robot_ip)); + ur_driver_.reset(new UrDriver(robot_ip, script_filename)); if (!root_nh.getParam("hardware_interface/joints", joint_names_)) { diff --git a/ur_rtde_driver/src/ros/hardware_interface_node.cpp b/ur_rtde_driver/src/ros/hardware_interface_node.cpp index ed3790b..24a9298 100644 --- a/ur_rtde_driver/src/ros/hardware_interface_node.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface_node.cpp @@ -64,7 +64,11 @@ int main(int argc, char** argv) hw_interface.reset(new ur_driver::HardwareInterface); - hw_interface->init(nh, nh_priv); + if (!hw_interface->init(nh, nh_priv)) + { + ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting"); + exit(1); + } ROS_INFO_STREAM("initialized hw interface"); controller_manager::ControllerManager cm(hw_interface.get(), nh); ROS_INFO_STREAM("started controller manager"); diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index 56060d1..f7a9eb5 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -41,85 +41,8 @@ static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}"); static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); -static const std::string POSITION_PROGRAM = R"( -def myProg(): - global steptime = get_steptime() - textmsg("steptime=", steptime) - MULT_jointstate = {{JOINT_STATE_REPLACE}} - SERVO_UNINITIALIZED = -1 - SERVO_IDLE = 0 - SERVO_RUNNING = 1 - cmd_servo_state = SERVO_UNINITIALIZED - cmd_servo_q = get_actual_joint_positions() - cmd_servo_q_last = get_actual_joint_positions() - def set_servo_setpoint(q): - enter_critical - cmd_servo_state = SERVO_RUNNING - cmd_servo_q_last = cmd_servo_q - cmd_servo_q = q - exit_critical - end - - def extrapolate(): - enter_critical - diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]] - cmd_servo_q_last = cmd_servo_q - cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]] - exit_critical - - return cmd_servo_q - end - - thread servoThread(): - state = SERVO_IDLE - while True: - enter_critical - q = cmd_servo_q - do_extrapolate = False - if (cmd_servo_state == SERVO_IDLE): - do_extrapolate = True - end - state = cmd_servo_state - cmd_servo_state = SERVO_IDLE - exit_critical - if do_extrapolate: - textmsg("No new setpoint received. Extrapolating.") - q = extrapolate() - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) - elif state == SERVO_RUNNING: - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) - else: - textmsg("Should not be here") - sync() - end - end - stopj(0.1) - end - socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") - - thread_servo = run servoThread() - keepalive = -2 - params_mult = socket_read_binary_integer(6+1, "reverse_socket") - keepalive = params_mult[7] - while keepalive > 0: - params_mult = socket_read_binary_integer(6+1, "reverse_socket", steptime) - if params_mult[0] > 0: - keepalive = params_mult[7] - 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] - set_servo_setpoint(q) - else: - # TODO: Extrapolation goes here - keepalive = keepalive - 1 - end - end - sleep(.1) - socket_close() - kill thread_servo -end -)"; - -ur_driver::UrDriver::UrDriver(const std::string& robot_ip) +ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file) : servoj_time_(0.008), servoj_gain_(750), servoj_lookahead_time_(0.03) { LOG_INFO("Initializing RTDE client"); @@ -141,7 +64,7 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip) uint32_t reverse_port = 50001; // TODO: Make this a parameter - std::string prog = POSITION_PROGRAM; + std::string prog = readScriptFile(script_file); prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); std::ostringstream out; out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; @@ -201,4 +124,12 @@ bool UrDriver::writeJointCommand(const vector6d_t& values) return true; } + +std::string UrDriver::readScriptFile(const std::string& filename) +{ + std::ifstream ifs(filename); + std::string content((std::istreambuf_iterator(ifs)), (std::istreambuf_iterator())); + + return content; +} } // namespace ur_driver