yarp-devices
exampleOnlineTrajectoryRemotePull.py
See also
exampleOnlineTrajectoryRemotePull.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 attending a remote callback')
14 parser.add_argument('--robot', default='/teo', type=str, help='robot port')
15 parser.add_argument('--part', default='/leftArm', type=str, help='part port')
16 parser.add_argument('--joint', default=5, type=int, help='joint id')
17 parser.add_argument('--speed', default=2.0, type=float, help='trajectory speed (deg/s)')
18 parser.add_argument('--target', default=-20.0, type=float, help='target position (deg)')
19 args = parser.parse_args()
20 
21 class SyncCallback(yarp.BottleCallback):
22  def __init__(self, initialPos, speed, cmd):
23  super().__init__()
24  self.count = 0
25  self.e0 = initialPos
26  self.v = speed
27  self.offset = 0.0
28  self.command = cmd
29 
30  def onRead(self, b, reader):
31  if b.size() == 2:
32  currentTime = b.get(0).asInt32() + b.get(1).asInt32() * 1e-9
33 
34  if self.count == 0:
35  self.offset = currentTime
36 
37  t = currentTime - self.offset
38  e = self.e0 + self.v * t
39 
40  print('[%d] New target: %f' % (self.count, e))
41  self.command(e)
42  self.count += 1
43 
44 yarp.Network.init()
45 
46 if not yarp.Network.checkNetwork():
47  print('Please start a yarp name server first')
48  raise SystemExit
49 
50 options = yarp.Property()
51 options.put('device', 'remote_controlboard')
52 options.put('local', '/exampleOnlineTrajectoryRemotePull')
53 options.put('remote', args.robot + args.part)
54 
55 dd = yarp.PolyDriver(options)
56 
57 if not dd.isValid():
58  print('Remote device not available')
59  raise SystemExit
60 
61 mode = dd.viewIControlMode()
62 enc = dd.viewIEncoders()
63 posd = dd.viewIPositionDirect()
64 
65 if mode is None or enc is None or posd is None:
66  print('Unable to acquire robot interfaces')
67  raise SystemExit
68 
69 syncPort = yarp.BufferedPortBottle()
70 syncPort.setReadOnly()
71 
72 if not syncPort.open('/exampleOnlineTrajectoryRemotePull/sync:i'):
73  print('Unable to open local sync port')
74  raise SystemExit
75 
76 if not yarp.Network.connect(args.robot + '/sync:o', syncPort.getName(), 'fast_tcp'):
77  print('Unable to connect to remote sync port')
78  raise SystemExit
79 
80 if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT):
81  print('Unable to set position direct mode')
82  raise SystemExit
83 
84 time.sleep(0.1) # hacky, but we need to make sure remote data arrived prior to calling getEncoder()
85 initialPos = enc.getEncoder(args.joint)
86 
87 print('Current enc value: %d' % initialPos)
88 input('Press ENTER to start')
89 print('Moving joint %d to %d degrees...' % (args.joint, args.target))
90 
91 distance = args.target - initialPos
92 velocity = math.copysign(args.speed, distance)
93 
94 callback = SyncCallback(initialPos, velocity, lambda pos: posd.setPosition(args.joint, pos))
95 syncPort.useCallback(callback)
96 
97 lastRef = yarp.DVector(1)
98 
99 while posd.getRefPosition(args.joint, lastRef) and abs(lastRef[0] - initialPos) < abs(distance):
100  time.sleep(0.01)
101 
102 syncPort.interrupt()
103 syncPort.close()
104 
105 dd.close()
106 yarp.Network.fini()
Definition: exampleOnlineTrajectoryRemotePull.cpp:64