Commit 51b52d62 authored by Felix Mauch's avatar Felix Mauch Committed by Felix Exner
Browse files

add support for speedj

parent acfffd42
......@@ -60,3 +60,58 @@ pos_traj_controller:
stop_trajectory_duration: 0.5
state_publish_rate: *loop_hz
action_monitor_rate: 10
vel_based_pos_traj_controller:
type: velocity_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
action_monitor_rate: 10
gains:
#!!These values have not been optimized!!
shoulder_pan_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
shoulder_lift_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
elbow_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_1_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_2_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
wrist_3_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1}
# Use a feedforward term to reduce the size of PID gains
velocity_ff:
shoulder_pan_joint: 1.0
shoulder_lift_joint: 1.0
elbow_joint: 1.0
wrist_1_joint: 1.0
wrist_2_joint: 1.0
wrist_3_joint: 1.0
# state_publish_rate: 50 # Defaults to 50
# action_monitor_rate: 20 # Defaults to 20
#stop_trajectory_duration: 0 # Defaults to 0.0
# Pass an array of joint velocities directly to the joints
joint_group_vel_controller:
type: velocity_controllers/JointGroupVelocityController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
......@@ -203,11 +203,11 @@ protected:
ur_controllers::ScaledPositionJointInterface spj_interface_;
hardware_interface::PositionJointInterface pj_interface_;
ur_controllers::SpeedScalingInterface speedsc_interface_;
// hardware_interface::VelocityJointInterface vj_interface_;
hardware_interface::VelocityJointInterface vj_interface_;
hardware_interface::ForceTorqueSensorInterface fts_interface_;
vector6d_t joint_position_command_;
// std::vector<double> joint_velocity_command_;
vector6d_t joint_velocity_command_;
vector6d_t joint_positions_;
vector6d_t joint_velocities_;
vector6d_t joint_efforts_;
......@@ -247,6 +247,7 @@ protected:
uint32_t runtime_state_;
bool position_controller_running_;
bool velocity_controller_running_;
PausingState pausing_state_;
double pausing_ramp_up_increment_;
......
<?xml version="1.0"?>
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur10_controllers.yaml"/>
<arg name="robot_description_file" default="$(find ur_description)/launch/ur10_upload.launch"/>
<arg name="kinematics_config" default="$(find ur_description)/config/ur10_default.yaml"/>
<arg name="robot_ip"/>
<arg name="limited" default="false"/>
<arg name="tf_prefix" default="" />
<arg name="controllers" default="joint_state_controller joint_group_vel_controller force_torque_sensor_controller"/>
<arg name="stopped_controllers" default="vel_based_pos_traj_controller"/>
<include file="$(find ur_robot_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>
<arg name="use_tool_communication" value="false"/>
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<arg name="robot_description_file" value="$(arg robot_description_file)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="limited" value="$(arg limited)"/>
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="controllers" value="$(arg controllers)"/>
<arg name="stopped_controllers" value="$(arg stopped_controllers)"/>
</include>
</launch>
......@@ -13,9 +13,9 @@
<arg name="script_sender_port" default="50002" doc="The driver will offer an interface to receive the program's URScript on this port. If the robot cannot connect to this port, `External Control` will stop immediately."/>
<arg name="kinematics_config" doc="Kinematics config file used for calibration correction. This will be used to verify the robot's calibration is matching the robot_description. Pass the same config file that is passed to the robot_description."/>
<arg name="tf_prefix" default="" doc="tf_prefix used for the robot."/>
<arg name="controllers" default="joint_state_controller scaled_pos_traj_controller speed_scaling_state_controller force_torque_sensor_controller" doc="Controllers that are activated by default."/>
<arg name="stopped_controllers" default="pos_traj_controller" doc="Controllers that are initally loaded, but not started."/>
<arg name="urscript_file" default="$(find ur_robot_driver)/resources/servoj.urscript" doc="Path to URScript that will be sent to the robot and that forms the main control program."/>
<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="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}}
global steptime = get_steptime()
global speedj_time = steptime - 0.1*steptime
textmsg("steptime=", steptime)
MULT_jointstate = {{JOINT_STATE_REPLACE}}
global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
cmd_speedj_active = True
keepalive = -2
def set_speed(qd):
enter_critical
cmd_servo_qd = qd
cmd_speedj_active = True
exit_critical
end
thread speedThread():
while True:
enter_critical
qd = cmd_servo_qd
exit_critical
#textmsg("loop")
if cmd_speedj_active:
# Having this shortly under steptime seems to help
speedj(qd, 40.0, speedj_time)
else:
stopj(5.0)
end
sync()
end
stopj(5.0)
end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
thread_speed = run speedThread()
params_mult = socket_read_binary_integer(1+6, "reverse_socket")
keepalive = params_mult[1]
while keepalive > 0:
params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02)
if params_mult[0] > 0:
keepalive = params_mult[1]
if keepalive > 1:
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)
#speedj(qd, 40.0, 0.007)
else:
cmd_speedj_active = False
keepalive = 1
end
else:
textmsg("Missing package from remote pc")
keepalive = keepalive - 1
end
end
sleep(.1)
socket_close()
kill thread_speed
......@@ -35,6 +35,7 @@ namespace ur_driver
{
HardwareInterface::HardwareInterface()
: joint_position_command_({ 0, 0, 0, 0, 0, 0 })
, joint_velocity_command_({ 0, 0, 0, 0, 0, 0 })
, joint_positions_{ { 0, 0, 0, 0, 0, 0 } }
, joint_velocities_{ { 0, 0, 0, 0, 0, 0 } }
, joint_efforts_{ { 0, 0, 0, 0, 0, 0 } }
......@@ -44,6 +45,7 @@ HardwareInterface::HardwareInterface()
, safety_mode_(ur_dashboard_msgs::SafetyMode::NORMAL)
, runtime_state_(static_cast<uint32_t>(rtde_interface::RUNTIME_STATE::STOPPED))
, position_controller_running_(false)
, velocity_controller_running_(false)
, pausing_state_(PausingState::RUNNING)
, pausing_ramp_up_increment_(0.01)
, controllers_initialized_(false)
......@@ -281,6 +283,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
// Create joint position control interface
pj_interface_.registerHandle(
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i]));
vj_interface_.registerHandle(
hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
}
......@@ -295,6 +299,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
registerInterface(&js_interface_);
registerInterface(&spj_interface_);
registerInterface(&pj_interface_);
registerInterface(&vj_interface_);
registerInterface(&speedsc_interface_);
registerInterface(&fts_interface_);
......@@ -473,6 +478,10 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
{
ur_driver_->writeJointCommand(joint_position_command_);
}
else if (velocity_controller_running_)
{
ur_driver_->writeJointCommand(joint_velocity_command_);
}
else if (robot_program_running_)
{
ur_driver_->writeKeepalive();
......@@ -511,6 +520,7 @@ void HardwareInterface::doSwitch(const std::list<hardware_interface::ControllerI
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
position_controller_running_ = false;
velocity_controller_running_ = false;
for (auto& controller_it : start_list)
{
for (auto& resource_it : controller_it.claimed_resources)
......@@ -523,6 +533,10 @@ void HardwareInterface::doSwitch(const std::list<hardware_interface::ControllerI
{
position_controller_running_ = true;
}
if (resource_it.hardware_interface == "hardware_interface::VelocityJointInterface")
{
velocity_controller_running_ = true;
}
}
}
}
......
......@@ -87,13 +87,27 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
std::string local_ip = rtde_client_->getIP();
std::string prog = readScriptFile(script_file);
prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE));
while (prog.find(JOINT_STATE_REPLACE) != std::string::npos)
{
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_;
prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
while (prog.find(SERVO_J_REPLACE) != std::string::npos)
{
prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
}
while (prog.find(SERVER_IP_REPLACE) != std::string::npos)
{
prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
}
while (prog.find(SERVER_PORT_REPLACE) != std::string::npos)
{
prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
}
robot_version_ = rtde_client_->getVersion();
......
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