23 | * The application provides a {@link RoboticsAPITask#initialize()} and a 24 | * {@link RoboticsAPITask#run()} method, which will be called successively in 25 | * the application lifecycle. The application will terminate automatically after 26 | * the {@link RoboticsAPITask#run()} method has finished or after stopping the 27 | * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an 28 | * exception is thrown during initialization or run. 29 | *
30 | * It is imperative to call super.dispose() when overriding the
31 | * {@link RoboticsAPITask#dispose()} method.
32 | *
33 | * @see #initialize()
34 | * @see #run()
35 | * @see #dispose()
36 | */
37 | public class SmartServoControl extends RoboticsAPIApplication {
38 | // members
39 | private LBR _theLbr;
40 | /**
41 | * Will be initialized from the routine "createTool()".
42 | *
43 | * IMPORTANT NOTE: Set the load mass properly
44 | */
45 | private IDirectServoRuntime theSmartServoRuntime = null;
46 | private int _count = 0;
47 | // private JointStreamer js;
48 | private JointTargetsListner jt;
49 |
50 | @Override
51 | public void initialize()
52 | {
53 | getContext().dumpDevices();
54 |
55 | // Locate the "first" Lightweight Robot in the system
56 | _theLbr = ServoMotionUtilities.locateLBR(getContext());
57 | //js = new JointStreamer();
58 | jt = new JointTargetsListner();
59 | }
60 |
61 | /**
62 | * Move to an initial Position WARNING: MAKE SHURE, THAT the pose is
63 | * collision free.
64 | */
65 | public void moveToInitialPosition()
66 | {
67 | _theLbr.move(
68 | ptp(0., 0., 0., 0, 0.,
69 | 0., 0.).setJointVelocityRel(0.1));
70 |
71 | /*
72 | *
73 | * For Completeness Sake, the validation is performed here Even it would
74 | * not be necessary within this sample.
75 | *
76 | * As long, as you'd remain within position control, the validation is
77 | * not necessary ... but, lightweight robot without ImpedanceControl is
78 | * a car without fuel...
79 | *
80 | * Note: The Validation itself justifies, that in this very time
81 | * instance, the load parameter setting was sufficient. This does not
82 | * mean by far, that the parameter setting is valid in the sequel or
83 | * lifetime of this program
84 |
85 | try
86 | {
87 | if (!SmartServo.validateForImpedanceMode(_toolAttachedToLBR))
88 | {
89 | System.out
90 | .println("Validation of Torque Model failed - correct your mass property settings");
91 | System.out
92 | .println("SmartServo will be available for position controlled mode only, until validation is performed");
93 | }
94 | }
95 | catch (IllegalStateException e)
96 | {
97 | System.out.println("Omitting validation failure for this sample\n"
98 | + e.getMessage());
99 | }
100 | */
101 | }
102 |
103 | // Sleep in between
104 | //private int _milliSleepToEmulateComputationalEffort = 10;//000;
105 | //private int _numRuns = 10000;
106 | //private double _amplitude = 0.2;
107 | //private double _freqency = 0.1;
108 | //private int steps = 0;
109 |
110 | @Override
111 | public void run()
112 | {
113 |
114 | jt.start();
115 |
116 | //moveToInitialPosition();
117 |
118 | JointPosition initialPosition = new JointPosition(
119 | _theLbr.getCurrentJointPosition());
120 | DirectServo aSmartServoMotion = new DirectServo(initialPosition);
121 |
122 | //aSmartServoMotion.overrideJointAcceleration(1);
123 | //aSmartServoMotion.setJointJerk(1);
124 |
125 | double exec_time = 100e-3;
126 | aSmartServoMotion.setMinimumTrajectoryExecutionTime(exec_time);
127 | aSmartServoMotion.setTimeoutAfterGoalReach(3000);
128 | aSmartServoMotion.setJointVelocityRel(0.2);
129 | aSmartServoMotion.setJointAccelerationRel(0.2);
130 | //aSmartServoMotion.setSpeedTimeoutAfterGoalReach(80e-3);
131 |
132 | System.out.println("Starting SmartServo in Position Mode");
133 | _theLbr.moveAsync(aSmartServoMotion);
134 |
135 | // Fetch the Runtime of the Motion part
136 | theSmartServoRuntime = aSmartServoMotion.getRuntime();
137 |
138 |
139 | //theSmartServoRuntime.activateVelocityPlanning(true);
140 |
141 | // create an JointPosition Instance, to play with
142 | JointPosition destination = new JointPosition(
143 | _theLbr.getJointCount());
144 | JointPosition target_vel = new JointPosition(
145 | _theLbr.getJointCount());
146 | System.out.println("start loop");
147 | // For Roundtrip time measurement...
148 | //StatisticTimer timing = new StatisticTimer();
149 | try
150 | {
151 |
152 | double kp[] = new double[7]; //0.005;
153 | double kd[] = new double[7];
154 | double amp[] = new double[7];
155 | int joint_id = 6;
156 | kp[6] = 0.1;//0.04;
157 | kd[6] = 0.0;
158 | amp[6] = 2; //5
159 | kp[5] = 0.04;
160 | kd[5] = 0;
161 | amp[5] = 2; //2
162 | kp[4] = 0.06;//0.05;
163 | kd[4] = 0;
164 | amp[4] = 2;
165 | kp[3] = 0.07;//0.003;
166 | kd[3] = 0;
167 | amp[3] = 2;
168 | kp[2] = 0.05;//0.02;
169 | kd[2] = 0;
170 | amp[2] = 2;
171 | kp[1] = 0.02;//0.003;
172 | kd[1] = 0;
173 | amp[1] = 2;
174 | kp[0] = 0.06;//0.04;
175 | kd[0] = 0;
176 | amp[0] = 2;
177 | int step_ctr=0;
178 | System.out.println("starting servoing");
179 | long t_last =System.nanoTime();
180 | long t_here = t_last;
181 |
182 | JointPosition curMsrJntPose = theSmartServoRuntime
183 | .getAxisQMsrOnController();
184 | StatisticTimer timing = new StatisticTimer();
185 | JointPosition prevMsrJntPose = new JointPosition(curMsrJntPose);
186 | JointPosition prevErr = new JointPosition(curMsrJntPose);
187 |
188 | boolean isFirstStep = true;
189 | while(true) {
190 |
191 | OneTimeStep aStep = timing.newTimeStep();
192 | //System.out.println("asking for joints");
193 | theSmartServoRuntime.updateWithRealtimeSystem();
194 | curMsrJntPose = theSmartServoRuntime
195 | .getAxisQMsrOnController();
196 |
197 |
198 | target_vel = jt.getJP(curMsrJntPose);
199 |
200 | // theSmartServoRuntime.updateWithRealtimeSystem();
201 | // curMsrJntPose = theSmartServoRuntime
202 | // .getAxisQMsrOnController();
203 |
204 |
205 | t_here = System.nanoTime();
206 | double dt = (t_here-t_last)/1e9;
207 | t_last = t_here;
208 | //double t = t_here/1e9;
209 | //if time step is too large top
210 | if(dt > 0.2) dt = 0;
211 |
212 | JointPosition curr_vel = new JointPosition(target_vel);
213 |
214 | JointPosition e = new JointPosition(target_vel);
215 |
216 | //JointPosition u = new JointPosition(target_vel);
217 | for (int i=0; i<_theLbr.getJointCount(); ++i) {
218 | curr_vel.set(i, (curMsrJntPose.get(i)-prevMsrJntPose.get(i))/dt);
219 | prevMsrJntPose.set(i,curMsrJntPose.get(i));
220 | e.set(i,target_vel.get(i)-curr_vel.get(i));
221 | if(isFirstStep) {
222 | prevErr.set(i,e.get(i));
223 | }
224 | destination.set(i, kp[i]*e.get(i) + curMsrJntPose.get(i) + amp[i]*dt*target_vel.get(i));
225 | prevErr.set(i,e.get(i));
226 |
227 | if(destination.get(i)-curMsrJntPose.get(i) < Math.toRadians(-4)) {
228 | destination.set(i,curMsrJntPose.get(i)+Math.toRadians(-4));
229 | }
230 | if(destination.get(i)-curMsrJntPose.get(i) > Math.toRadians(4)) {
231 | destination.set(i,curMsrJntPose.get(i)+Math.toRadians(4));
232 | }
233 | /*if(Math.abs(destination.get(i)-curMsrJntPose.get(i)) < Math.toRadians(0.1)) {
234 | destination.set(i,curMsrJntPose.get(i));
235 | }*/
236 |
237 | }
238 | isFirstStep=false;
239 |
240 | //aSmartServoMotion.setMinimumTrajectoryExecutionTime(dt/1000.);
241 | // for (int i=0; i<_theLbr.getJointCount(); ++i) {
242 | //calculate q_k+1 HERE
243 |
244 | //double jerk = 2*(target_vel.get(i) - curr_vel.get(i))/(exec_time*exec_time);
245 | //destination.set(i, curMsrJntPose.get(i)+exec_time*target_vel.get(i)+jerk/6*Math.pow(exec_time, 3));
246 | // destination.set(i, curMsrJntPose.get(i)+exec_time*target_vel.get(i));
247 |
248 |
249 | //}
250 | //System.out.println("sent to jp "+destination.get(6));
251 |
252 | //aSmartServoMotion.setMinimumTrajectoryExecutionTime(1.5*dt);
253 | theSmartServoRuntime.setDestination(destination);
254 |
255 |
256 | if(step_ctr%100 ==0) {
257 | // step_ctr=0;
258 | //for (int i=5; i<_theLbr.getJointCount(); ++i) {
259 |
260 | System.out.println("sent to jp "+joint_id+": "+destination.get(joint_id)+" dt is "+dt+"target vel is "+target_vel.get(joint_id)+" curr vel "+curr_vel.get(joint_id)+" curr pos "+curMsrJntPose.get(joint_id));
261 | //}
262 | getLogger().info("Statistic Timing of Overall Loop " + timing);
263 |
264 | }
265 | step_ctr++;
266 |
267 | //theSmartServoRuntime.setDestination(curMsrJntPose,target_vel);
268 |
269 | // ThreadUtil.milliSleep(10);
270 |
271 | aStep.end();
272 |
273 |
274 |
275 | //js.setJS(curMsrJntPose);
276 |
277 | }
278 | }
279 | catch (Exception e)
280 | {
281 | System.out.println(e);
282 | e.printStackTrace();
283 | }
284 | ThreadUtil.milliSleep(1000);
285 | // Stop the motion
286 | theSmartServoRuntime.stopMotion();
287 | try {
288 | jt.join();
289 | } catch (InterruptedException e) {
290 | // TODO Auto-generated catch block
291 | e.printStackTrace();
292 | }
293 |
294 |
295 | }
296 |
297 |
298 |
299 |
300 | /**
301 | * Auto-generated method stub. Do not modify the contents of this method.
302 | */
303 | public static void main(String[] args) {
304 | SmartServoControl app = new SmartServoControl();
305 | app.runApplication();
306 | }
307 | }
308 |
--------------------------------------------------------------------------------
/lbr_iiwa_description/urdf/lbr_iiwa.xacro:
--------------------------------------------------------------------------------
1 |
2 |