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()
22 def __init__(self, initialPos, speed, cmd):
30 def onRead(self, b, reader):
32 currentTime = b.get(0).asInt32() + b.get(1).asInt32() * 1e-9
35 self.offset = currentTime
37 t = currentTime - self.offset
38 e = self.e0 + self.v * t
40 print(
'[%d] New target: %f' % (self.count, e))
46 if not yarp.Network.checkNetwork():
47 print(
'Please start a yarp name server first')
50 options = yarp.Property()
51 options.put(
'device',
'remote_controlboard')
52 options.put(
'local',
'/exampleOnlineTrajectoryRemotePull')
53 options.put(
'remote', args.robot + args.part)
55 dd = yarp.PolyDriver(options)
58 print(
'Remote device not available')
61 mode = dd.viewIControlMode()
62 enc = dd.viewIEncoders()
63 posd = dd.viewIPositionDirect()
65 if mode
is None or enc
is None or posd
is None:
66 print(
'Unable to acquire robot interfaces')
69 syncPort = yarp.BufferedPortBottle()
70 syncPort.setReadOnly()
72 if not syncPort.open(
'/exampleOnlineTrajectoryRemotePull/sync:i'):
73 print(
'Unable to open local sync port')
76 if not yarp.Network.connect(args.robot +
'/sync:o', syncPort.getName(),
'fast_tcp'):
77 print(
'Unable to connect to remote sync port')
80 if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT):
81 print(
'Unable to set position direct mode')
85 initialPos = enc.getEncoder(args.joint)
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))
91 distance = args.target - initialPos
92 velocity = math.copysign(args.speed, distance)
94 callback =
SyncCallback(initialPos, velocity,
lambda pos: posd.setPosition(args.joint, pos))
95 syncPort.useCallback(callback)
97 lastRef = yarp.DVector(1)
99 while posd.getRefPosition(args.joint, lastRef)
and abs(lastRef[0] - initialPos) < abs(distance):
Definition: exampleOnlineTrajectoryRemotePull.cpp:64