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()
24 if not yarp.Network.checkNetwork():
25 print(
'Please start a yarp name server first')
28 options = yarp.Property()
29 options.put(
'device',
'remote_controlboard')
30 options.put(
'local',
'/exampleOfflineTrajectoryAsync')
31 options.put(
'remote', args.remote)
33 dd = yarp.PolyDriver(options)
36 print(
'Remote device not available')
39 mode = dd.viewIControlMode()
40 enc = dd.viewIEncoders()
41 posd = dd.viewIPositionDirect()
42 var = dd.viewIRemoteVariables()
44 if mode
is None or enc
is None or posd
is None or var
is None:
45 print(
'Unable to acquire robot interfaces')
49 p.put(
'ipMode', args.ip)
50 p.put(
'enableIp',
True)
53 b.addList().fromString(p.toString())
55 if not var.setRemoteVariable(
'all', b):
56 print(
'Unable to set interpolation mode')
59 if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT):
60 print(
'Unable to set position direct mode')
64 initialPos = enc.getEncoder(args.joint)
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))
70 distance = args.target - initialPos
71 increment = math.copysign(args.speed * args.period * 0.001, distance)
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)
83 time.sleep(args.period * 0.001 - ((time.time() - start) % (args.period * 0.001)))