Commit 6b818d98 authored by Felix Mauch's avatar Felix Mauch Committed by Felix Exner
Browse files

Send control type from hardware interface

TODO:
 - Documentation of function members
 - Using enums for control modes
parent 8840d3f7
......@@ -37,6 +37,18 @@ namespace ur_driver
{
namespace comm
{
/*!
* \brief Control modes as interpreted from the script runnning on the robot.
*/
enum class ControlMode : int32_t
{
MODE_STOPPED = -2, ///< When this is set, the program is expected to stop and exit.
MODE_UNINITIALIZED = -1, ///< Startup default until another mode is sent to the script.
MODE_IDLE = 0, ///< Set when no controller is currently active controlling the robot.
MODE_SERVOJ = 1, ///< Set when servoj control is active.
MODE_SPEEDJ = 2 ///< Set when speedj control is active.
};
/*!
* \brief The ReverseInterface class handles communication to the robot. It starts a server and
* waits for the robot to connect via its URCaps program.
......@@ -72,17 +84,19 @@ public:
/*!
* \brief Writes needed information to the robot to be read by the URCaps program.
*
* \param positions A vector of joint position targets for the robot
* \param type An additional integer used to command the program to end when needed
* \param positions A vector of joint targets for the robot
* \param control_mode Control mode assigned to this command. See documentation of ::ControlMode
* for details on possible values.
*
* \returns True, if the write was performed successfully, false otherwise.
*/
bool write(const vector6d_t* positions, const int32_t type = 2)
bool write(const vector6d_t* positions, const ControlMode control_mode = ControlMode::MODE_IDLE)
{
uint8_t buffer[sizeof(uint32_t) * 7];
uint8_t buffer[sizeof(int32_t) * 8];
uint8_t* b_pos = buffer;
int32_t val = htobe32(type);
// The first element is always the keepalive signal.
int32_t val = htobe32(1);
b_pos += append(b_pos, val);
if (positions != nullptr)
......@@ -94,6 +108,13 @@ public:
b_pos += append(b_pos, val);
}
}
else
{
b_pos += 6 * sizeof(int32_t);
}
val = htobe32(toUnderlying(control_mode));
b_pos += append(b_pos, val);
size_t written;
......
......@@ -46,4 +46,17 @@ std::ostream& operator<<(std::ostream& out, const std::array<T, N>& item)
out << "]";
return out;
}
/*!
* \brief Converts an enum type to its underlying type
*
* \param e Enum value that should be converted
*
* \returns Enum value converted to underlying type
*/
template <typename E>
constexpr typename std::underlying_type<E>::type toUnderlying(const E e) noexcept
{
return static_cast<typename std::underlying_type<E>::type>(e);
}
} // namespace ur_driver
......@@ -122,10 +122,11 @@ public:
* the robot.
*
* \param values Desired joint positions
* \param control_mode Control mode this command is assigned to.
*
* \returns True on successful write.
*/
bool writeJointCommand(const vector6d_t& values);
bool writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode);
/*!
* \brief Write a keepalive signal only.
......
......@@ -15,7 +15,7 @@
<arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller vel_based_pos_traj_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="joint_group_vel_controller"/>
<arg name="urscript_file" default="$(find ur_robot_driver)/resources/speedj.urscript"/>
<arg name="urscript_file" default="$(find ur_robot_driver)/resources/servoj.urscript"/>
<arg name="rtde_output_recipe_file" default="$(find ur_robot_driver)/resources/rtde_output_recipe.txt" doc="Recipe file used for the RTDE-outputs. Only change this if you know what you're doing."/>
<arg name="rtde_input_recipe_file" default="$(find ur_robot_driver)/resources/rtde_input_recipe.txt" doc="Recipe file used for the RTDE-inputs. Only change this if you know what you're doing."/>
<arg name="tool_voltage" default="0" doc="Tool voltage set at the beginning of the UR program. Only used, when `use_tool_communication` is set to true."/>
......
{{BEGIN_REPLACE}}
steptime = get_steptime()
speedj_time = steptime - 0.1*steptime
textmsg("steptime=", steptime)
MULT_jointstate = {{JOINT_STATE_REPLACE}}
#Constants
SERVO_STOPPED = -2
SERVO_UNINITIALIZED = -1
SERVO_IDLE = 0
SERVO_RUNNING = 1
MODE_STOPPED = -2
MODE_UNINITIALIZED = -1
MODE_IDLE = 0
MODE_SERVOJ = 1
MODE_SPEEDJ = 2
#Global variables are also showed in the Teach pendants variable list
global cmd_servo_state = SERVO_UNINITIALIZED
global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
global cmd_servo_q = get_actual_joint_positions()
global cmd_servo_q_last = get_actual_joint_positions()
global extrapolate_count = 0
global extrapolate_max_count = 0
global control_mode = MODE_UNINITIALIZED
cmd_speedj_active = True
def set_servo_setpoint(q):
cmd_servo_state = SERVO_RUNNING
......@@ -32,8 +41,9 @@ def extrapolate():
end
thread servoThread():
textmsg("Starting servo thread")
state = SERVO_IDLE
while state > SERVO_STOPPED:
while control_mode == MODE_SERVOJ:
enter_critical
q = cmd_servo_q
do_extrapolate = False
......@@ -57,7 +67,6 @@ thread servoThread():
elif state == SERVO_RUNNING:
extrapolate_count = 0
servoj(q, t=steptime, {{SERVO_J_REPLACE}})
else:
extrapolate_count = 0
sync()
......@@ -67,24 +76,55 @@ thread servoThread():
textmsg("servo thread ended")
stopj(0.1)
end
# Helpers for speed control
def set_speed(qd):
cmd_servo_qd = qd
control_mode = MODE_SPEEDJ
end
thread speedThread():
textmsg("Starting speed thread")
while control_mode == MODE_SPEEDJ:
enter_critical
qd = cmd_servo_qd
exit_critical
# Having this shortly under steptime seems to help
speedj(qd, 40.0, speedj_time)
sync()
end
textmsg("speedj thread ended")
stopj(5.0)
end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
thread_servo = run servoThread()
control_mode = MODE_UNINITIALIZED
thread_move = 0
keepalive = -2
textmsg("External control active")
params_mult = socket_read_binary_integer(1+6, "reverse_socket")
params_mult = socket_read_binary_integer(1+6+1, "reverse_socket")
keepalive = params_mult[1]
while keepalive > 0:
while keepalive > 0 and control_mode > MODE_STOPPED:
enter_critical
socket_send_line(1, "reverse_socket")
params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
if params_mult[0] > 0:
keepalive = params_mult[1]
if params_mult[1] > 1:
if control_mode != params_mult[8]:
control_mode = params_mult[8]
if control_mode == MODE_SERVOJ:
thread_move = run servoThread()
elif control_mode == MODE_SPEEDJ:
thread_move = run speedThread()
end
end
if control_mode == MODE_SERVOJ:
q = [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, params_mult[7] / MULT_jointstate]
set_servo_setpoint(q)
else:
set_servo_setpoint(get_actual_joint_positions())
elif control_mode == MODE_SPEEDJ:
qd = [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, params_mult[7] / MULT_jointstate]
set_speed(qd)
end
else:
keepalive = keepalive - 1
......@@ -92,8 +132,6 @@ while keepalive > 0:
exit_critical
end
textmsg("Stopping communication and servoing")
cmd_servo_state = SERVO_STOPPED
sleep(.1)
textmsg("Stopping communication and control")
sleep(0.1)
socket_close("reverse_socket")
kill thread_servo
......@@ -476,22 +476,22 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
{
if (position_controller_running_)
{
ur_driver_->writeJointCommand(joint_position_command_);
ur_driver_->writeJointCommand(joint_position_command_, comm::ControlMode::MODE_SERVOJ);
}
else if (velocity_controller_running_)
{
ur_driver_->writeJointCommand(joint_velocity_command_);
}
else if (robot_program_running_)
{
ur_driver_->writeKeepalive();
ur_driver_->writeJointCommand(joint_velocity_command_, comm::ControlMode::MODE_SPEEDJ);
}
else
{
ur_driver_->stopControl();
ur_driver_->writeKeepalive();
}
packet_read_ = false;
}
else
{
ur_driver_->stopControl();
}
}
bool HardwareInterface::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
......
......@@ -167,11 +167,11 @@ std::unique_ptr<rtde_interface::DataPackage> ur_driver::UrDriver::getDataPackage
return rtde_client_->getDataPackage(timeout);
}
bool UrDriver::writeJointCommand(const vector6d_t& values)
bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMode control_mode)
{
if (reverse_interface_active_)
{
return reverse_interface_->write(&values);
return reverse_interface_->write(&values, control_mode);
}
return false;
}
......@@ -181,7 +181,7 @@ bool UrDriver::writeKeepalive()
if (reverse_interface_active_)
{
vector6d_t* fake = nullptr;
return reverse_interface_->write(fake, 1);
return reverse_interface_->write(fake, comm::ControlMode::MODE_IDLE);
}
return false;
}
......@@ -196,7 +196,7 @@ bool UrDriver::stopControl()
if (reverse_interface_active_)
{
vector6d_t* fake = nullptr;
return reverse_interface_->write(fake, 0);
return reverse_interface_->write(fake, comm::ControlMode::MODE_STOPPED);
}
return false;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment