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()
23 if not yarp.Network.checkNetwork():
24 print(
'Please start a yarp name server first')
27 options = yarp.Property()
28 options.put(
'device',
'remote_controlboard')
29 options.put(
'local',
'/exampleOnlineTrajectoryRemotePush')
30 options.put(
'remote', args.remote)
32 dd = yarp.PolyDriver(options)
35 print(
'Remote device not available')
38 mode = dd.viewIControlMode()
39 enc = dd.viewIEncoders()
40 posd = dd.viewIPositionDirect()
42 if mode
is None or enc
is None or posd
is None:
43 print(
'Unable to acquire robot interfaces')
46 if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT):
47 print(
'Unable to set position direct mode')
51 initialPos = enc.getEncoder(args.joint)
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))
57 distance = args.target - initialPos
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)
68 time.sleep(args.period * 0.001 - ((time.time() - start) % (args.period * 0.001)))