Commit 8840d3f7 authored by Felix Mauch's avatar Felix Mauch Committed by Felix Exner
Browse files

added speed controllers to all robots and added ur10e_speed launchfile

parent 51b52d62
......@@ -63,13 +63,7 @@ pos_traj_controller:
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
joints: *robot_joints
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
......@@ -107,11 +101,4 @@ vel_based_pos_traj_controller:
# 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
joints: *robot_joints
......@@ -60,3 +60,45 @@ 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: *robot_joints
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: *robot_joints
......@@ -60,3 +60,46 @@ 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: *robot_joints
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: *robot_joints
......@@ -60,3 +60,45 @@ 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: *robot_joints
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: *robot_joints
......@@ -60,3 +60,45 @@ 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: *robot_joints
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: *robot_joints
......@@ -60,3 +60,45 @@ 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: *robot_joints
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: *robot_joints
<?xml version="1.0"?>
<launch>
<!-- GDB functionality -->
<arg name="debug" default="false" />
<arg name="use_tool_communication" default="false"/>
<arg name="controller_config_file" default="$(find ur_robot_driver)/config/ur10e_controllers.yaml"/>
<arg name="robot_description_file" default="$(find ur_e_description)/launch/ur10e_upload.launch"/>
<arg name="kinematics_config" default="$(find ur_e_description)/config/ur10e_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"/>
<arg name="tool_voltage" default="0"/>
<arg name="tool_parity" default="0"/>
<arg name="tool_baud_rate" default="115200"/>
<arg name="tool_stop_bits" default="1"/>
<arg name="tool_rx_idle_chars" default="1.5"/>
<arg name="tool_tx_idle_chars" default="3.5"/>
<arg name="tool_device_name" default="/tmp/ttyUR"/>
<arg name="tool_tcp_port" default="54321"/>
<include file="$(find ur_robot_driver)/launch/ur_common.launch">
<arg name="debug" value="$(arg debug)"/>
<arg name="use_tool_communication" value="$(arg use_tool_communication)"/>
<arg name="controller_config_file" value="$(arg controller_config_file)"/>
<arg name="robot_description_file" value="$(arg robot_description_file)"/>
<arg name="kinematics_config" value="$(arg kinematics_config)"/>
<arg name="robot_ip" value="$(arg robot_ip)"/>
<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)"/>
<arg name="tool_voltage" value="$(arg tool_voltage)"/>
<arg name="tool_parity" value="$(arg tool_parity)"/>
<arg name="tool_baud_rate" value="$(arg tool_baud_rate)"/>
<arg name="tool_stop_bits" value="$(arg tool_stop_bits)"/>
<arg name="tool_rx_idle_chars" value="$(arg tool_rx_idle_chars)"/>
<arg name="tool_tx_idle_chars" value="$(arg tool_tx_idle_chars)"/>
<arg name="tool_device_name" value="$(arg tool_device_name)"/>
<arg name="tool_tcp_port" value="$(arg tool_tcp_port)"/>
</include>
</launch>
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