rev: e9de9ca7b57e5f1fd12caf1e5bd5576e47093ce0 tukan/testing/test_bullet.sc -rw-r--r-- 6.9 KiB View raw Log this file
e9de9ca7b57e — Leonard Ritter * removed unused modules from test 5 months ago
                                                                                
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
171
172
173
174
175
176
using import C.string

import ..tukan.use
using import tukan.bullet.C

let datadir =
    .. module-dir "/../build/src/bullet/data"
local dofCount : i32
local posVarCount : i32
local sensorJointIndexLeft = -1
local sensorJointIndexRight = -1
let urdfFileName = (.. datadir "/r2d2.urdf")
let sdfFileName = (.. datadir "/kuka_iiwa/model.sdf")
let gravx gravy gravz = 0.0 0.0 -9.8
let timeStep = (/ 60.0)
local startPosX : f64
local startPosY : f64
local startPosZ : f64
local imuLinkIndex = -1
local bodyIndex = -1

let sm = (b3ConnectPhysicsDirect)
assert (b3CanSubmitCommand sm)

do
    let command = (b3InitPhysicsParamCommand sm)
    let ret = (b3PhysicsParamSetGravity command gravx gravy gravz)
    let ret = (b3PhysicsParamSetTimeStep command timeStep)
    let statusHandle = (b3SubmitClientCommandAndWaitStatus sm command)
    assert ((b3GetStatusType statusHandle) == CMD_CLIENT_COMMAND_COMPLETED)

do
    let command = (b3LoadSdfCommandInit sm sdfFileName)
    let statusHandle = (b3SubmitClientCommandAndWaitStatus sm command)
    let statusType = (b3GetStatusType statusHandle)
    assert (statusType == CMD_SDF_LOADING_COMPLETED)

    local bodyIndicesOut : (array i32 10) # MAX_SDF_BODIES = 10
    let numBodies = (b3GetStatusBodyIndices statusHandle bodyIndicesOut 10)
    assert (numBodies == 1)
    bodyUniqueId := bodyIndicesOut @ 0
    do
        let command = (b3InitRequestVisualShapeInformation sm bodyUniqueId)
        let statusHandle = (b3SubmitClientCommandAndWaitStatus sm command)
        let statusType = (b3GetStatusType statusHandle)
        if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
            local vi : b3VisualShapeInformation
            b3GetVisualShapeInformation sm &vi

    let numJoints = (b3GetNumJoints sm bodyUniqueId)
    assert (numJoints == 7)

    do
        let jointAngle = 0.0:f64
        let commandHandle = (b3CreatePoseCommandInit sm bodyUniqueId)
        for jointIndex in (range numJoints)
            b3CreatePoseCommandSetJointPosition sm commandHandle jointIndex jointAngle

        let statusHandle = (b3SubmitClientCommandAndWaitStatus sm commandHandle)

        assert ((b3GetStatusType statusHandle) == CMD_CLIENT_COMMAND_COMPLETED)

do
    let command = (b3LoadUrdfCommandInit sm urdfFileName)

    # setting the initial position, orientation and other arguments are optional
    startPosX = 2
    startPosY = 0
    startPosZ = 1
    let ret = (b3LoadUrdfCommandSetStartPosition command startPosX startPosY startPosZ)
    let statusHandle = (b3SubmitClientCommandAndWaitStatus sm command)
    let statusType = (b3GetStatusType statusHandle)
    assert (statusType == CMD_URDF_LOADING_COMPLETED)
    let bodyIndex = (b3GetStatusBodyIndex statusHandle)

    if (bodyIndex >= 0)
        let numJoints = (b3GetNumJoints sm bodyIndex)
        for i in (range numJoints)
            local jointInfo : b3JointInfo
            b3GetJointInfo sm bodyIndex i &jointInfo

            # printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
            # pick the IMU link index based on torso name
            if ((strstr jointInfo.m_linkName "base_link") != null)
                imuLinkIndex = i

            # pick the joint index based on joint name
            if ((strstr jointInfo.m_jointName "base_to_left_leg") != null)
                sensorJointIndexLeft = i

            if ((strstr jointInfo.m_jointName "base_to_right_leg") != null)
                sensorJointIndexRight = i

        if ((sensorJointIndexLeft >= 0) or (sensorJointIndexRight >= 0))
            let command = (b3CreateSensorCommandInit sm bodyIndex)
            if (imuLinkIndex >= 0)
                let ret = (b3CreateSensorEnableIMUForLink command imuLinkIndex 1)

            if (sensorJointIndexLeft >= 0)
                let ret = (b3CreateSensorEnable6DofJointForceTorqueSensor command sensorJointIndexLeft 1)

            if (sensorJointIndexRight >= 0)
                let ret = (b3CreateSensorEnable6DofJointForceTorqueSensor command sensorJointIndexRight 1)

            let statusHandle = (b3SubmitClientCommandAndWaitStatus sm command)
            assert ((b3GetStatusType statusHandle) == CMD_CLIENT_COMMAND_COMPLETED)

    do
        let command = (b3CreateBoxShapeCommandInit sm)
        let ret = (b3CreateBoxCommandSetStartPosition command 0 0 -1)
        let ret = (b3CreateBoxCommandSetStartOrientation command 0 0 0 1)
        let ret = (b3CreateBoxCommandSetHalfExtents command 10 10 1)
        let statusHandle = (b3SubmitClientCommandAndWaitStatus sm command)
        assert ((b3GetStatusType statusHandle) == CMD_RIGID_BODY_CREATION_COMPLETED)

    do
        let command = (b3RequestActualStateCommandInit sm bodyIndex)
        let statusHandle = (b3SubmitClientCommandAndWaitStatus sm command)
        let statusType = (b3GetStatusType statusHandle)
        assert (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)

        if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
            b3GetStatusActualState statusHandle
                                    \ null &posVarCount &dofCount
                                    \ null null null null
            assert (posVarCount == 15)
            assert (dofCount == 14)

    # perform some simulation steps for testing
    for i in (range 1000)
        assert (b3CanSubmitCommand sm)
        let statusHandle = (b3SubmitClientCommandAndWaitStatus sm (b3InitStepSimulationCommand sm))
        let statusType = (b3GetStatusType statusHandle)
        assert (statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED)

    do
        let width = 1024
        let height = 1024

        let command = (b3InitRequestCameraImage sm)

        b3RequestCameraImageSetPixelResolution command width height
        b3RequestCameraImageSelectRenderer command ER_BULLET_HARDWARE_OPENGL
        let statusHandle = (b3SubmitClientCommandAndWaitStatus sm command)

    do
        assert (b3CanSubmitCommand sm)
        let state = (b3SubmitClientCommandAndWaitStatus sm (b3RequestActualStateCommandInit sm bodyIndex))

        if (sensorJointIndexLeft >= 0)
            local sensorState : b3JointSensorState
            b3GetJointState sm state sensorJointIndexLeft &sensorState

            print "Sensor for joint" sensorJointIndexLeft "="
                deref (sensorState.m_jointForceTorque @ 0)
                deref (sensorState.m_jointForceTorque @ 1)
                deref (sensorState.m_jointForceTorque @ 2)

        if (sensorJointIndexRight >= 0)
            local sensorState : b3JointSensorState
            b3GetJointState sm state sensorJointIndexRight &sensorState

            print "Sensor for joint" sensorJointIndexRight "="
                deref (sensorState.m_jointForceTorque @ 0)
                deref (sensorState.m_jointForceTorque @ 1)
                deref (sensorState.m_jointForceTorque @ 2)

        let statusHandle = (b3SubmitClientCommandAndWaitStatus sm (b3InitResetSimulationCommand sm))
        assert ((b3GetStatusType statusHandle) == CMD_RESET_SIMULATION_COMPLETED)

b3DisconnectSharedMemory sm

print "OK."