Skip to content

Commit 62cfc2f

Browse files
WSQsGithubWSQsGithub
committed
first commit
0 parents  commit 62cfc2f

8 files changed

+1088
-0
lines changed

‎Floor/draw_grid.lua

+60
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
--[[
2+
Author: Siqi Wang [email protected]
3+
Date: 2023-09-02 20:49:51
4+
LastEditors: Siqi Wang [email protected]
5+
LastEditTime: 2023-09-02 20:50:01
6+
FilePath: \script\scripts\Floor\draw_grid.lua
7+
Description: This is a script for drawing grids on the floor
8+
9+
Copyright (c) 2023 by Siqi Wang, All Rights Reserved.
10+
--]]
11+
12+
13+
-- This function is called once at the start of the simulation
14+
function sysCall_init()
15+
-- Get the floor object
16+
local floor = sim.getObjectHandle('Floor') -- 'Floor' is the name of the floor object; change it according to your scene
17+
18+
-- Set the parameters for the grid lines
19+
local gridSize = 10 -- Grid size
20+
local gridSpacing = 1.0 -- Grid spacing
21+
local gridColor = {0.5, 0.5, 0.5} -- Grid line color (gray)
22+
23+
-- Set the line width (you can adjust this value)
24+
local lineWidth = 0.05 -- Line width
25+
26+
-- Set the start Z coordinate (you can adjust this value)
27+
local startZ = 0.003 -- Start Z coordinate
28+
29+
-- Create the grid lines
30+
for i = -gridSize / 2, gridSize / 2, gridSpacing do
31+
local startX = -gridSize / 2
32+
local startY = i
33+
local endX = gridSize / 2
34+
local endY = i
35+
36+
-- Create a line object with the specified line width
37+
local line = sim.addDrawingObject(sim.drawing_lines, lineWidth, 0, floor, 1, gridColor)
38+
39+
-- Set the coordinates of the line with the specified start Z coordinate
40+
sim.addDrawingObjectItem(line, {startX, startY, startZ, endX, endY, startZ})
41+
end
42+
43+
for i = -gridSize / 2, gridSize / 2, gridSpacing do
44+
local startX = i
45+
local startY = -gridSize / 2
46+
local endX = i
47+
local endY = gridSize / 2
48+
49+
-- Create a line object with the specified line width
50+
local line = sim.addDrawingObject(sim.drawing_lines, lineWidth, 0, floor, 1, gridColor)
51+
52+
-- Set the coordinates of the line with the specified start Z coordinate
53+
sim.addDrawingObjectItem(line, {startX, startY, startZ, endX, endY, startZ})
54+
end
55+
end
56+
57+
-- This function is called at the end of the simulation
58+
function sysCall_cleanup()
59+
-- Clean up any resources if needed
60+
end

‎Omniplatform/omniplatform_steering.lua

+160
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
--[[
2+
Author: Siqi Wang [email protected]
3+
Date: 2023-09-02 18:48:02
4+
LastEditors: Siqi Wang [email protected]
5+
LastEditTime: 2023-09-02 19:27:23
6+
FilePath: \script\ready\omniPlatform_steering.lua
7+
Description: A non-threading keyboard steering implementation of the OmniPlation
8+
9+
Copyright (c) 2023 by Siqi Wang, All Rights Reserved.
10+
--]]
11+
12+
13+
function sysCall_init()
14+
-- Retrieve handles and prepare initial parameters:
15+
pathVisible = true
16+
platform=sim.getObject('.')
17+
omniPads={}
18+
for i=1,4,1 do
19+
omniPads[i]=sim.getObject('./link['..(i-1)..']/regularRotation')
20+
print(omniPads)
21+
end
22+
v=80*2.398795*math.pi/180-- 2.398795 is a factor needed to obtain the right pad rotation velocity
23+
24+
if (pathVisible) then
25+
-- visualization cold:
26+
graph=sim.getObject('/Graph')
27+
-- Define the X/Y graph curved:
28+
objectPosX=sim.addGraphStream(graph,'object pos x','m',1)
29+
objectPosY=sim.addGraphStream(graph,'object pos y','m',1)
30+
sim.addGraphCurve(graph,'object pos x/y',2,{objectPosX,objectPosY},{0,0},'m by m')
31+
-- Define a drawing object:
32+
drawing=sim.addDrawingObject(sim.drawing_linestrip,2,0,-1,0,{1,0,1})
33+
-- Define a structure that holds the recoded data:
34+
recording={}
35+
end
36+
37+
end
38+
39+
function sysCall_actuation()
40+
pos = sim.getObjectPosition(platform,sim.handle_world)
41+
if (pathVisible) then
42+
-- visualization code
43+
sim.setGraphStreamValue(graph,objectPosX,pos[1])
44+
sim.setGraphStreamValue(graph,objectPosY,pos[2])
45+
-- Add another point to the drawing object:
46+
sim.addDrawingObjectItem(drawing,pos)
47+
-- Record that point:
48+
recording[#recording+1]=pos
49+
end
50+
51+
message,auxiliaryData=sim.getSimulatorMessage()
52+
while message~=-1 do
53+
if (message==sim.message_keypress) then
54+
print(auxiliaryData)
55+
if (auxiliaryData[1]==49) then
56+
goSouthWest(v)
57+
end
58+
if (auxiliaryData[1]==50) then
59+
goSouth(v)
60+
end
61+
if (auxiliaryData[1]==51) then
62+
goSouthEast(v)
63+
end
64+
if (auxiliaryData[1]==52) then
65+
goWest(v)
66+
end
67+
if (auxiliaryData[1]==53) then
68+
stayStill()
69+
end
70+
if (auxiliaryData[1]==54) then
71+
goEast(v)
72+
end
73+
if (auxiliaryData[1]==55) then
74+
goNorthWest(v)
75+
end
76+
if (auxiliaryData[1]==56) then
77+
goNorth(v)
78+
end
79+
if (auxiliaryData[1]==57) then
80+
goNorthEast(v)
81+
end
82+
end
83+
message,auxiliaryData=sim.getSimulatorMessage()
84+
end
85+
86+
end
87+
88+
function goNorth(v)
89+
sim.setJointTargetVelocity(omniPads[1],v)
90+
sim.setJointTargetVelocity(omniPads[2],-v)
91+
sim.setJointTargetVelocity(omniPads[3],-v)
92+
sim.setJointTargetVelocity(omniPads[4],v)
93+
end
94+
95+
function goSouth(v)
96+
sim.setJointTargetVelocity(omniPads[1],-v)
97+
sim.setJointTargetVelocity(omniPads[2],v)
98+
sim.setJointTargetVelocity(omniPads[3],v)
99+
sim.setJointTargetVelocity(omniPads[4],-v)
100+
end
101+
102+
function goWest(v)
103+
sim.setJointTargetVelocity(omniPads[1],v)
104+
sim.setJointTargetVelocity(omniPads[2],v)
105+
sim.setJointTargetVelocity(omniPads[3],-v)
106+
sim.setJointTargetVelocity(omniPads[4],-v)
107+
end
108+
109+
function goEast(v)
110+
sim.setJointTargetVelocity(omniPads[1],-v)
111+
sim.setJointTargetVelocity(omniPads[2],-v)
112+
sim.setJointTargetVelocity(omniPads[3],v)
113+
sim.setJointTargetVelocity(omniPads[4],v)
114+
end
115+
116+
function goNorthWest(v)
117+
sim.setJointTargetVelocity(omniPads[1],2*v)
118+
sim.setJointTargetVelocity(omniPads[2],0)
119+
sim.setJointTargetVelocity(omniPads[3],-2*v)
120+
sim.setJointTargetVelocity(omniPads[4],0)
121+
end
122+
123+
function goSouthEast(v)
124+
sim.setJointTargetVelocity(omniPads[1],-2*v)
125+
sim.setJointTargetVelocity(omniPads[2],0)
126+
sim.setJointTargetVelocity(omniPads[3],2*v)
127+
sim.setJointTargetVelocity(omniPads[4],0)
128+
end
129+
130+
function goNorthEast(v)
131+
sim.setJointTargetVelocity(omniPads[1],0)
132+
sim.setJointTargetVelocity(omniPads[2],-2*v)
133+
sim.setJointTargetVelocity(omniPads[3],0)
134+
sim.setJointTargetVelocity(omniPads[4],2*v)
135+
end
136+
137+
function goSouthWest(v)
138+
sim.setJointTargetVelocity(omniPads[1],0)
139+
sim.setJointTargetVelocity(omniPads[2],2*v)
140+
sim.setJointTargetVelocity(omniPads[3],0)
141+
sim.setJointTargetVelocity(omniPads[4],-2*v)
142+
end
143+
144+
145+
146+
147+
function stayStill()
148+
sim.setJointTargetVelocity(omniPads[1],0)
149+
sim.setJointTargetVelocity(omniPads[2],0)
150+
sim.setJointTargetVelocity(omniPads[3],0)
151+
sim.setJointTargetVelocity(omniPads[4],0)
152+
end
153+
154+
155+
function stay()
156+
sim.setJointTargetVelocity(omniPads[1],0)
157+
sim.setJointTargetVelocity(omniPads[2],0)
158+
sim.setJointTargetVelocity(omniPads[3],0)
159+
sim.setJointTargetVelocity(omniPads[4],0)
160+
end

0 commit comments

Comments
 (0)