Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 01:50:46 +02:00
Code Releases Activity

Load the script from an external file

Browse Source
This commit is contained in:
Felix Mauch
2019-05-20 12:28:35 +02:00
parent ec31db35b2
commit 89ee1aae8a
7 changed files with 110 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h
View File

@@ -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_interface::RTDEClient> rtde_client_;

4
ur_rtde_driver/launch/ur10_ros_control.launch
View File

@@ -18,6 +18,7 @@
<arg name="shutdown_on_disconnect" default="true" />
<arg name="controllers" default="joint_state_controller pos_based_pos_traj_controller speed_scaling_state_controller"/>
<arg name="stopped_controllers" default=""/>
<arg name="urscript_file" default="$(find ur_rtde_driver)/resources/servoj.urscript"/>
<!-- robot model -->
<include file="$(find ur_description)/launch/ur10_upload.launch">
<arg name="limited" value="$(arg limited)"/>
@@ -25,8 +26,9 @@
<!-- Load hardware interface -->
<node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)">
<node name="ur_hardware_interface" pkg="ur_rtde_driver" type="ur_rtde_driver_node" output="screen" launch-prefix="$(arg launch_prefix)" required="true">
<param name="robot_ip" type="str" value="$(arg robot_ip)"/>
<param name="script_file" value="$(arg urscript_file)"/>
<!--<param name="reverse_port" type="int" value="$(arg reverse_port)" />-->
<!--<param name="min_payload" type="double" value="$(arg min_payload)"/>-->
<!--<param name="max_payload" type="double" value="$(arg max_payload)"/>-->

76
ur_rtde_driver/resources/servoj.urscript Normal file
View File

@@ -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

9
ur_rtde_driver/src/main_plain_driver.cpp
View File

@@ -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)
{

8
ur_rtde_driver/src/ros/hardware_interface.cpp
View File

@@ -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<std::string>("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_))
{

6
ur_rtde_driver/src/ros/hardware_interface_node.cpp
View File

@@ -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");

89
ur_rtde_driver/src/ur/ur_driver.cpp
View File

@@ -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<char>(ifs)), (std::istreambuf_iterator<char>()));
return content;
}
} // namespace ur_driver