13 parser = argparse.ArgumentParser(description=
'perform an offline trajectory via position commands with fixed 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',
'/exampleOfflineTrajectorySync')
31 options.put(
'remote', args.remote)
32 options.put(
'writeStrict',
'on')
34 dd = yarp.PolyDriver(options)
37 print(
'Remote device not available')
40 mode = dd.viewIControlMode()
41 enc = dd.viewIEncoders()
42 pos = dd.viewIPositionControl()
43 posd = dd.viewIPositionDirect()
44 var = dd.viewIRemoteVariables()
46 if mode
is None or enc
is None or pos
is None or posd
is None or var
is None:
47 print(
'Unable to acquire robot interfaces')
51 p.put(
'ipMode', args.ip)
52 p.put(
'ipPeriodMs', args.period)
53 p.put(
'enableIp',
True)
56 b.addList().fromString(p.toString())
58 if not var.setRemoteVariable(
'all', b):
59 print(
'Unable to set interpolation mode')
62 if not mode.setControlMode(args.joint, yarp.VOCAB_CM_POSITION_DIRECT):
63 print(
'Unable to set position direct mode')
67 initialPos = enc.getEncoder(args.joint)
69 print(
'Current enc value: %d' % initialPos)
70 input(
'Press ENTER to start')
71 print(
'Moving joint %d to %d degrees...' % (args.joint, args.target))
73 distance = args.target - initialPos
74 increment = math.copysign(args.speed * args.period * 0.001, distance)
78 while abs(distance) > abs(newDistance):
79 newDistance = count * increment
80 position = initialPos + newDistance
81 print(
"[%d] New target: %f" % (count, position))
82 posd.setPosition(args.joint, position)
85 motionDone = yarp.BVector(1)
87 while pos.checkMotionDone(args.joint, motionDone)
and motionDone[0]: