yarp-devices
exampleOfflineTrajectorySync.py
See also
exampleOfflineTrajectorySync.cpp
Tutorial: Trajectory Execution (external)
1 
8 import argparse
9 import math
10 import time
11 import yarp
12 
13 parser = argparse.ArgumentParser(description='perform an offline trajectory via position commands with fixed period')
14 parser.add_argument('--remote', default='/teo/leftArm', type=str, help='remote port')
15 parser.add_argument('--joint', default=5, type=int, help='joint id')
16 parser.add_argument('--speed', default=2.0, type=float, help='trajectory speed (deg/s)')
17 parser.add_argument('--target', default=-20.0, type=float, help='target position (deg)')
18 parser.add_argument('--period', default=50, type=int, help='command period (ms)')
19 parser.add_argument('--ip', default='pt', type=str, help='interpolation submode [pt|pvt]')
20 args = parser.parse_args()
21 
22 yarp.Network.init()
23 
24 if not yarp.Network.checkNetwork():
25  print('Please start a yarp name server first')
26  raise SystemExit
27 
28 options = yarp.Property()
29 options.put('device', 'remote_controlboard')
30 options.put('local', '/exampleOfflineTrajectorySync')
31 options.put('remote', args.remote)
32 options.put('writeStrict', 'on')
33 
34 dd = yarp.PolyDriver(options)
35 
36 if not dd.isValid():
37  print('Remote device not available')
38  raise SystemExit
39 
40 mode = dd.viewIControlMode()
41 enc = dd.viewIEncoders()
42 pos = dd.viewIPositionControl()
43 posd = dd.viewIPositionDirect()
44 var = dd.viewIRemoteVariables()
45 
46 if mode is None or enc is None or pos is None or posd is None or var is None:
47  print('Unable to acquire robot interfaces')
48  raise SystemExit
49 
50 p = yarp.Property()
51 p.put('ipMode', args.ip)
52 p.put('ipPeriodMs', args.period)
53 p.put('enableIp', True) # important: place this last
54 
55 b = yarp.Bottle()
56 b.addList().fromString(p.toString()) # additional nesting because of controlboardremapper
57 
58 if not var.setRemoteVariable('all', b):
59  print('Unable to set interpolation mode')
60  raise SystemExit
61 
62 if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT):
63  print('Unable to set position direct mode')
64  raise SystemExit
65 
66 time.sleep(0.1) # hacky, but we need to make sure remote data arrived prior to calling getEncoder()
67 initialPos = enc.getEncoder(args.joint)
68 
69 print('Current enc value: %d' % initialPos)
70 input('Press ENTER to start')
71 print('Moving joint %d to %d degrees...' % (args.joint, args.target))
72 
73 distance = args.target - initialPos
74 increment = math.copysign(args.speed * args.period * 0.001, distance)
75 count = 1
76 newDistance = 0.0
77 
78 while abs(distance) > abs(newDistance):
79  newDistance = count * increment
80  position = initialPos + newDistance
81  print("[%d] New target: %f" % (count, position))
82  posd.setPosition(args.joint, position)
83  count += 1
84 
85 motionDone = yarp.BVector(1)
86 
87 while pos.checkMotionDone(args.joint, motionDone) and motionDone[0]:
88  print('.', end='')
89  time.sleep(0.1)
90 
91 print(' end')
92 
93 dd.close()
94 yarp.Network.fini()