-
Notifications
You must be signed in to change notification settings - Fork 195
/
driver_qos.lua
executable file
·68 lines (54 loc) · 1.71 KB
/
driver_qos.lua
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
#!/usr/bin/lua
-- Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
-- All rights reserved.
--
-- This software may be modified and distributed under the terms of the
-- BSD-3-Clause license. See the accompanying LICENSE file for details.
-- LUA_CPATH should have the path to yarp-lua binding library (i.e. yarp.so, yarp.dll)
require("yarp")
-- initialize yarp network
yarp.Network()
options = yarp.Property()
options:put("device", "remote_controlboard")
options:put("local", "/motor/lua/right_arm")
options:put("remote", "/icubSim/right_arm")
-- setting QoS pereferences for the driver (local and remote)
qos_local = options:addGroup("local_qos")
qos_local:put("thread_priority", 20)
qos_local:put("thread_policy", 1)
qos_local:put("packet_priority", "LEVEL:HIGH")
qos_remote = options:addGroup("remote_qos")
qos_remote:put("thread_priority", 30)
qos_remote:put("thread_policy", 1)
qos_remote:put("packet_priority", "DSCP:AF42")
-- open the driver
driver = yarp.PolyDriver(options)
if driver:isValid() == false then
print("Cannot open the device");
os.exit()
end
-- open the interfaces
ipos = driver:viewIPositionControl()
if ipos == nil then
print("Cannot open the IPositionControl interface");
driver:close()
os.exit()
end
icm = driver:viewIControlMode2()
if icm == nil then
print("Cannot open the IControlMode2 interface");
driver:close()
os.exit()
end
-- send position command to motors
for i=1,100 do
position = math.random(-90, 10)
print("setting position of joint 0 to "..position)
icm:setControlMode(0, VOCAB_CM_POSITION)
ipos:setRefSpeed(0, 30)
ipos:positionMove(0, position)
yarp.delay(0.5)
end
driver:close()
-- Deinitialize yarp network
yarp.Network_fini()