test_bullet.sc

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."