yarp-devices
exampleOnlineTrajectoryRemotePush.py
See also
exampleOnlineTrajectoryRemotePush.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 online trajectory via position commands, pushing new setpoints at the sender\'s discretion')
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 args = parser.parse_args()
20 
21 yarp.Network.init()
22 
23 if not yarp.Network.checkNetwork():
24  print('Please start a yarp name server first')
25  raise SystemExit
26 
27 options = yarp.Property()
28 options.put('device', 'remote_controlboard')
29 options.put('local', '/exampleOnlineTrajectoryRemotePush')
30 options.put('remote', args.remote)
31 
32 dd = yarp.PolyDriver(options)
33 
34 if not dd.isValid():
35  print('Remote device not available')
36  raise SystemExit
37 
38 mode = dd.viewIControlMode()
39 enc = dd.viewIEncoders()
40 posd = dd.viewIPositionDirect()
41 
42 if mode is None or enc is None or posd is None:
43  print('Unable to acquire robot interfaces')
44  raise SystemExit
45 
46 if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT):
47  print('Unable to set position direct mode')
48  raise SystemExit
49 
50 time.sleep(0.1) # hacky, but we need to make sure remote data arrived prior to calling getEncoder()
51 initialPos = enc.getEncoder(args.joint)
52 
53 print('Current enc value: %d' % initialPos)
54 input('Press ENTER to start')
55 print('Moving joint %d to %d degrees...' % (args.joint, args.target))
56 
57 distance = args.target - initialPos
58 current = initialPos
59 i = 1
60 start = time.time()
61 
62 while abs(distance) > abs(current - initialPos):
63  current = current + math.copysign(args.speed * args.period * 0.001, distance)
64  print("[%d] New target: %f" % (i, current))
65  posd.setPosition(args.joint, current)
66  i += 1
67  # https://stackoverflow.com/a/25251804/10404307
68  time.sleep(args.period * 0.001 - ((time.time() - start) % (args.period * 0.001)))
69 
70 dd.close()
71 yarp.Network.fini()