diff --git a/ur_rtde_driver/config/ur10e_controllers.yaml b/ur_rtde_driver/config/ur10e_controllers.yaml index c100dfc..9b71f28 100644 --- a/ur_rtde_driver/config/ur10e_controllers.yaml +++ b/ur_rtde_driver/config/ur10e_controllers.yaml @@ -72,3 +72,58 @@ pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 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 + diff --git a/ur_rtde_driver/config/ur3_controllers.yaml b/ur_rtde_driver/config/ur3_controllers.yaml index ebbd6be..b8059a6 100644 --- a/ur_rtde_driver/config/ur3_controllers.yaml +++ b/ur_rtde_driver/config/ur3_controllers.yaml @@ -72,3 +72,59 @@ pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 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 + diff --git a/ur_rtde_driver/config/ur3e_controllers.yaml b/ur_rtde_driver/config/ur3e_controllers.yaml index c100dfc..9b71f28 100644 --- a/ur_rtde_driver/config/ur3e_controllers.yaml +++ b/ur_rtde_driver/config/ur3e_controllers.yaml @@ -72,3 +72,58 @@ pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 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 + diff --git a/ur_rtde_driver/config/ur5_controllers.yaml b/ur_rtde_driver/config/ur5_controllers.yaml index ebbd6be..c6a7a54 100644 --- a/ur_rtde_driver/config/ur5_controllers.yaml +++ b/ur_rtde_driver/config/ur5_controllers.yaml @@ -72,3 +72,58 @@ pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 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 + diff --git a/ur_rtde_driver/config/ur5e_controllers.yaml b/ur_rtde_driver/config/ur5e_controllers.yaml index c100dfc..9b71f28 100644 --- a/ur_rtde_driver/config/ur5e_controllers.yaml +++ b/ur_rtde_driver/config/ur5e_controllers.yaml @@ -72,3 +72,58 @@ pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 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 + diff --git a/ur_rtde_driver/launch/ur10e_speed_bringup.launch b/ur_rtde_driver/launch/ur10e_speed_bringup.launch new file mode 100644 index 0000000..b9bb6a1 --- /dev/null +++ b/ur_rtde_driver/launch/ur10e_speed_bringup.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +