1242dc7d5e98 — Leonard Ritter 3 months ago
* ported `test.c` from bullet to scopes
2 files changed, 173 insertions(+), 3 deletions(-)

M testing/test_bullet.sc
M tukan/bullet/C.sc
M testing/test_bullet.sc +170 -1
@@ 3,6 3,7 @@ using import glm
 using import Capture
 using import Array
 using import enum
+using import C.string
 
 import ..tukan.use
 using import tukan.VertexPainter

          
@@ 12,8 13,176 @@ using import tukan.bullet.C
 using import tukan.rotation
 using import tukan.math
 
-let api = (b3ConnectPhysicsDirect)
+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."
+
+;
 
 #
     global world = (World)

          
M tukan/bullet/C.sc +3 -2
@@ 16,10 16,11 @@ include
     """"#include "bullet/PhysicsClientC_API.h"
 
 do
-    #using lib.define filter "^(B3(.+))$"
-    #using lib.const filter "^(b3(.+))$"
+    #using lib.define filter "^(CMD(.+))$"
+    using lib.const filter "^(CMD_(.+)|ER_(.+))$"
     using lib.typedef filter "^(b3(.+))$"
     using lib.extern filter "^(b3(.+))$"
+    using lib.struct filter "^(b3(.+))$"
 
     #@@ 'on stage.on-init
     #inline ()