ur10_controllers.yaml 5.06 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
# Settings for ros_control control loop
hardware_control_loop:
   loop_hz: &loop_hz 125

# Settings for ros_control hardware interface
hardware_interface:
   joints: &robot_joints
     - shoulder_pan_joint
     - shoulder_lift_joint
     - elbow_joint
     - wrist_1_joint
     - wrist_2_joint
     - wrist_3_joint

# Publish all joint states ----------------------------------
joint_state_controller:
   type:         joint_state_controller/JointStateController
   publish_rate: *loop_hz

# Publish wrench ----------------------------------
force_torque_sensor_controller:
   type:         force_torque_sensor_controller/ForceTorqueSensorController
   publish_rate: *loop_hz

# Publish speed_scaling factor
speed_scaling_state_controller:
   type:         ur_controllers/SpeedScalingStateController
   publish_rate: *loop_hz

# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
scaled_pos_traj_controller:
   type: position_controllers/ScaledJointTrajectoryController
   joints: *robot_joints
   constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
      elbow_joint: {trajectory: 0.2, goal: 0.1}
      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
   stop_trajectory_duration: 0.5
   state_publish_rate: *loop_hz
   action_monitor_rate: 10

pos_traj_controller:
   type: position_controllers/JointTrajectoryController
   joints: *robot_joints
   constraints:
      goal_time: 0.6
      stopped_velocity_tolerance: 0.05
      shoulder_pan_joint: {trajectory: 0.2, goal: 0.1}
      shoulder_lift_joint: {trajectory: 0.2, goal: 0.1}
      elbow_joint: {trajectory: 0.2, goal: 0.1}
      wrist_1_joint: {trajectory: 0.2, goal: 0.1}
      wrist_2_joint: {trajectory: 0.2, goal: 0.1}
      wrist_3_joint: {trajectory: 0.2, goal: 0.1}
   stop_trajectory_duration: 0.5
   state_publish_rate: *loop_hz
   action_monitor_rate: 10

scaled_vel_traj_controller:
   type: velocity_controllers/ScaledJointTrajectoryController
   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:  *loop_hz
   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

vel_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:  *loop_hz
   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