yarp-devices
exampleOfflineTrajectoryAsync.py
See also
exampleOfflineTrajectoryAsync.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 a variable 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', '/exampleOfflineTrajectoryAsync')
31 options.put('remote', args.remote)
32 
33 dd = yarp.PolyDriver(options)
34 
35 if not dd.isValid():
36  print('Remote device not available')
37  raise SystemExit
38 
39 mode = dd.viewIControlMode()
40 enc = dd.viewIEncoders()
41 posd = dd.viewIPositionDirect()
42 var = dd.viewIRemoteVariables()
43 
44 if mode is None or enc is None or posd is None or var is None:
45  print('Unable to acquire robot interfaces')
46  raise SystemExit
47 
48 p = yarp.Property()
49 p.put('ipMode', args.ip)
50 p.put('enableIp', True) # important: place this last
51 
52 b = yarp.Bottle()
53 b.addList().fromString(p.toString()) # additional nesting because of controlboardremapper
54 
55 if not var.setRemoteVariable('all', b):
56  print('Unable to set interpolation mode')
57  raise SystemExit
58 
59 if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT):
60  print('Unable to set position direct mode')
61  raise SystemExit
62 
63 time.sleep(0.1) # hacky, but we need to make sure remote data arrived prior to calling getEncoder()
64 initialPos = enc.getEncoder(args.joint)
65 
66 print('Current enc value: %d' % initialPos)
67 input('Press ENTER to start')
68 print('Moving joint %d to %d degrees...' % (args.joint, args.target))
69 
70 distance = args.target - initialPos
71 increment = math.copysign(args.speed * args.period * 0.001, distance)
72 count = 1
73 newDistance = 0.0
74 start = time.time()
75 
76 while abs(distance) > abs(newDistance):
77  newDistance = count * increment
78  position = initialPos + newDistance
79  print("[%d] New target: %f" % (count, position))
80  posd.setPosition(args.joint, position)
81  count += 1
82  # https://stackoverflow.com/a/25251804/10404307
83  time.sleep(args.period * 0.001 - ((time.time() - start) % (args.period * 0.001)))
84 
85 dd.close()
86 yarp.Network.fini()