lua/defaultMainScript-2.lua at master · CoppeliaRobotics/lua · GitHub

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
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
sim = require('sim-2')
-- This is the main script. The main script is not supposed to be modified,
-- unless there is a very good reason to do it.
-- Without main script, there is no simulation.

function sysCall_init()
    sim.handleSimulationStart()
end

function sysCall_actuation()
    sim.scene:handleSimulationScripts(sim.syscb_actuation)
    sim.scene:handleCustomizationScripts(sim.syscb_actuation)
    sim.app:handleAddOnScripts(sim.syscb_actuation)
    sim.app:handleSandboxScript(sim.syscb_actuation)
    sim.handleJointMotion()
    sim.handleDynamics(sim.getSimulationTimeStep())
end

function sysCall_sensing()
    sim.handleSensingStart()
    local proxSensors = sim.scene:getObjects({'proximitySensor'})
    for i = 1, #proxSensors do
        local s = proxSensors[i]
        if not s.explicitHandling then
            s:handleSensor()
        end
    end
    local visionSensors = sim.scene:getObjects({'visionSensor'})
    for i = 1, #visionSensors do
        local s = visionSensors[i]
        if not s.explicitHandling then
            s:handleSensor()
        end
    end
    sim.scene:handleSimulationScripts(sim.syscb_sensing)
    sim.scene:handleCustomizationScripts(sim.syscb_sensing)
    sim.app:handleAddOnScripts(sim.syscb_sensing)
    sim.app:handleSandboxScript(sim.syscb_sensing)
end

function sysCall_cleanup()
    sim.scene:handleSimulationScripts(sim.syscb_cleanup)
    local proxSensors = sim.scene:getObjects({'proximitySensor'})
    for i = 1, #proxSensors do
        local s = proxSensors[i]
        if not s.explicitHandling then
            s:resetSensor()
        end
    end
    local visionSensors = sim.scene:getObjects({'visionSensor'})
    for i = 1, #visionSensors do
        local s = visionSensors[i]
        if not s.explicitHandling then
            s:resetSensor()
        end
    end
end

function sysCall_suspend()
    sim.scene:handleSimulationScripts(sim.syscb_suspend)
    sim.scene:handleCustomizationScripts(sim.syscb_suspend)
    sim.app:handleAddOnScripts(sim.syscb_suspend)
    sim.app:handleSandboxScript(sim.syscb_suspend)
end

function sysCall_suspended()
    sim.scene:handleSimulationScripts(sim.syscb_suspended)
    sim.scene:handleCustomizationScripts(sim.syscb_suspended)
    sim.app:handleAddOnScripts(sim.syscb_suspended)
    sim.app:handleSandboxScript(sim.syscb_suspended)
end

function sysCall_resume()
    sim.scene:handleSimulationScripts(sim.syscb_resume)
    sim.scene:handleCustomizationScripts(sim.syscb_resume)
    sim.app:handleAddOnScripts(sim.syscb_resume)
    sim.app:handleSandboxScript(sim.syscb_resume)
end

function sysCall_joint(inData)
    if inData.mode == sim.jointmode_kinematic then
        if _S.kinJointMotionData == nil then _S.kinJointMotionData = {} end
        if _S.kinJointMotionData[inData.handle] == nil then
            _S.kinJointMotionData[inData.handle] = {vel = 0, accel = 0}
        end
        local joint = _S.kinJointMotionData[inData.handle]
        if inData.initVel then
            joint.vel = inData.initVel
            joint.accel = 0
        end
        local res, outData = pcall(jointKinematicMotion, joint, inData)
        return outData
    end
end

function jointKinematicMotion(joint, inData)
    local outData = nil
    local dt = sim.getSimulationTimeStep()
    local p = inData.pos
    if inData.targetPos then
        if ((inData.revolute == true) and (math.abs(inData.error) > 0.01 * math.pi / 180)) or
            ((inData.revolute == false) and (math.abs(inData.error) > 0.0001)) or (joint.vel ~= 0) then
            local rmlObject = sim.ruckigPos(
                                  1, 0.0001, -1, {p, joint.vel, joint.accel},
                                  {inData.maxVel, inData.maxAccel, inData.maxJerk}, {1},
                                  {p + inData.error, 0}
                              )
            local result, newPosVelAccel = sim.ruckigStep(rmlObject, dt)
            if result >= 0 then
                outData = {
                    pos = newPosVelAccel[1],
                    vel = newPosVelAccel[2],
                    accel = newPosVelAccel[3],
                }
                if result == 0 then
                    joint.vel = newPosVelAccel[2]
                    joint.accel = newPosVelAccel[3]
                else
                    joint.vel = 0
                    joint.accel = 0
                    outData = {immobile = true}
                end
            else
                joint.vel = 0
                joint.accel = 0
                outData = {immobile = true}
            end
            sim.ruckigRemove(rmlObject)
        end
    else
        if inData.targetVel ~= 0 or joint.vel ~= 0 or joint.accel ~= 0 then
            if inData.targetVel == joint.vel and joint.accel == 0 then
                outData = {pos = p + joint.vel * dt, vel = joint.vel, accel = 0.0}
            else
                local rmlObject = sim.ruckigVel(
                                      1, 0.0001, -1, {p, joint.vel, joint.accel},
                                      {inData.maxAccel, inData.maxJerk}, {1}, {inData.targetVel}
                                  )
                local result, newPosVelAccel, sync = sim.ruckigStep(rmlObject, dt)
                if result >= 0 then
                    if result == 0 then
                        outData = {
                            pos = newPosVelAccel[1],
                            vel = newPosVelAccel[2],
                            accel = newPosVelAccel[3],
                        }
                        joint.vel = newPosVelAccel[2]
                        joint.accel = newPosVelAccel[3]
                    else
                        local ddt = -sync -- vel. reached, we have some residual time
                        outData = {
                            pos = newPosVelAccel[1] + joint.vel * ddt,
                            vel = inData.targetVel,
                            accel = 0.0,
                        }
                        joint.vel = inData.targetVel
                        joint.accel = 0
                    end
                else
                    joint.vel = inData.targetVel
                    joint.accel = 0
                end
                sim.ruckigRemove(rmlObject)
            end
        else
            outData = {immobile = true}
        end
    end
    return outData
end