Exemple #1
0
    // Use this for initialization
    void Start()
    {
        //pybullet = NativeMethods.b3ConnectSharedMemory(NativeConstants.SHARED_MEMORY_KEY);
        //if (NativeMethods.b3CanSubmitCommand(pybullet)==0)
        //{
        pybullet = NativeMethods.b3ConnectPhysicsDirect();
        //}

        //NativeMethods.b3SetAdditionalSearchPath(pybullet, "d:\\");

        Debug.Log("can submit command " + NativeMethods.b3CanSubmitCommand(pybullet));


        IntPtr cmd    = NativeMethods.b3InitResetSimulationCommand(pybullet);
        IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
        EnumSharedMemoryServerStatus statusType1;
        {
            IntPtr command = NativeMethods.b3InitPhysicsParamCommand(pybullet);
            NativeMethods.b3PhysicsParamSetGravity(command, 0, -10, 0);
            IntPtr statusHandle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, command);
            statusType1 = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);
            Debug.Log("set grav status " + statusType1);
        }

        int numBodies = NativeMethods.b3GetNumBodies(pybullet);

        {
            cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf");
            Quaternion qq = Quaternion.Euler(-90, 0, 0);
            NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, qq.x, qq.y, qq.z, qq.w);
            status      = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
            statusType1 = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);
            Debug.Log("added plane " + statusType1);
        }

        {
            cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "quadruped/minitaur.urdf");
            NativeMethods.b3LoadUrdfCommandSetStartPosition(cmd, 0, 20, 0);
            Quaternion q = Quaternion.Euler(35, 0, 0);
            NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, q.x, q.y, q.z, q.w);
            status      = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
            statusType1 = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);
            Debug.Log("added minitaur " + statusType1);
        }

        m_cubeUid = NativeMethods.b3GetStatusBodyIndex(status);
        Debug.Log("minitaur " + m_cubeUid);
        EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);

        if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED)
        {
            numBodies = NativeMethods.b3GetNumBodies(pybullet);
            Debug.Log("Numbodies " + numBodies);
            cmd        = NativeMethods.b3InitRequestVisualShapeInformation(pybullet, m_cubeUid);
            status     = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
            statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);
            Debug.Log("StatusType " + statusType);
            if (statusType == EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_INFO_COMPLETED ||
                statusType == EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_UPDATE_COMPLETED)
            {
                b3VisualShapeInformation visuals = new b3VisualShapeInformation();
                NativeMethods.b3GetVisualShapeInformation(pybullet, ref visuals);
                Debug.Log("visuals.m_numVisualShapes=" + visuals.m_numVisualShapes);
                System.IntPtr visualPtr = visuals.m_visualShapeData;

                robot.bodies = new Body[visuals.m_numVisualShapes];

                for (int s = 0; s < visuals.m_numVisualShapes; s++)
                {
                    b3VisualShapeData visual = (b3VisualShapeData)Marshal.PtrToStructure(visualPtr, typeof(b3VisualShapeData));
                    visualPtr = new IntPtr(visualPtr.ToInt64() + (Marshal.SizeOf(typeof(b3VisualShapeData))));
                    Vector3 scale;
                    scale.x = (float)visual.m_dimensions[0];
                    scale.y = (float)visual.m_dimensions[1];
                    scale.z = (float)visual.m_dimensions[2];

                    Debug.Log("visual.m_visualGeometryType = " + (eUrdfGeomTypes)visual.m_visualGeometryType);
                    Debug.Log("visual.m_dimensions" + scale.x + "," + scale.y + "," + scale.z);

                    Vector3 pos;
                    pos.x = (float)visual.m_localVisualFrame[0];
                    pos.y = (float)visual.m_localVisualFrame[1];
                    pos.z = (float)visual.m_localVisualFrame[2];
                    Quaternion rot;
                    rot.x = (float)visual.m_localVisualFrame[3];
                    rot.y = (float)visual.m_localVisualFrame[4];
                    rot.z = (float)visual.m_localVisualFrame[5];
                    rot.w = (float)visual.m_localVisualFrame[6];

                    robot.bodies[s] = CreateShape(visual, "cube", pos, rot, scale);

                    if (visual.m_visualGeometryType == (int)eUrdfGeomTypes.GEOM_MESH)
                    {
                        Debug.Log("visual.m_meshAssetFileName=" + visual.m_meshAssetFileName);
                    }
                }
            }
            if (numBodies > 0)
            {
                b3BodyInfo info = new b3BodyInfo();
                NativeMethods.b3GetBodyInfo(pybullet, 0, ref info);
            }
        }
    }
Exemple #2
0
    // Use this for initialization
    void Start()
    {
        text = GetComponent <Text>();


        pybullet = NativeMethods.b3ConnectSharedMemory(NativeConstants.SHARED_MEMORY_KEY);
        if (NativeMethods.b3CanSubmitCommand(pybullet) == 0)
        {
            pybullet = NativeMethods.b3ConnectPhysicsDirect();
        }



        IntPtr cmd    = NativeMethods.b3InitResetSimulationCommand(pybullet);
        IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);

        {
            IntPtr command = NativeMethods.b3InitPhysicsParamCommand(pybullet);
            NativeMethods.b3PhysicsParamSetGravity(command, 0, -10, 0);
            IntPtr statusHandle = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, command);
        }

        int numBodies = NativeMethods.b3GetNumBodies(pybullet);

        Debug.Log(numBodies);
        {
            cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf");
            Quaternion qq = Quaternion.Euler(-90, 0, 0);
            NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, qq.x, qq.y, qq.z, qq.w);
            status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
        }


        cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "cube.urdf");
        NativeMethods.b3LoadUrdfCommandSetStartPosition(cmd, 0, 20, 0);
        Quaternion q = Quaternion.Euler(35, 0, 0);

        NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, q.x, q.y, q.z, q.w);
        status    = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
        m_cubeUid = NativeMethods.b3GetStatusBodyIndex(status);

        EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);

        if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED)
        {
            numBodies  = NativeMethods.b3GetNumBodies(pybullet);
            text.text  = numBodies.ToString();
            cmd        = NativeMethods.b3InitRequestVisualShapeInformation(pybullet, m_cubeUid);
            status     = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
            statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);

            if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_INFO_COMPLETED)
            {
                b3VisualShapeInformation visuals = new b3VisualShapeInformation();
                NativeMethods.b3GetVisualShapeInformation(pybullet, ref visuals);
                System.Console.WriteLine("visuals.m_numVisualShapes=" + visuals.m_numVisualShapes);
                System.IntPtr visualPtr = visuals.m_visualShapeData;

                for (int s = 0; s < visuals.m_numVisualShapes; s++)
                {
                    b3VisualShapeData visual = (b3VisualShapeData)Marshal.PtrToStructure(visualPtr, typeof(b3VisualShapeData));
                    visualPtr = new IntPtr(visualPtr.ToInt64() + (Marshal.SizeOf(typeof(b3VisualShapeData))));
                    double x = visual.m_dimensions[0];
                    double y = visual.m_dimensions[1];
                    double z = visual.m_dimensions[2];
                    System.Console.WriteLine("visual.m_visualGeometryType = " + visual.m_visualGeometryType);
                    System.Console.WriteLine("visual.m_dimensions" + x + "," + y + "," + z);
                    if (visual.m_visualGeometryType == (int)eUrdfGeomTypes.GEOM_MESH)
                    {
                        System.Console.WriteLine("visual.m_meshAssetFileName=" + visual.m_meshAssetFileName);
                        text.text = visual.m_meshAssetFileName;
                    }
                }
            }
            if (numBodies > 0)
            {
                b3BodyInfo info = new b3BodyInfo();
                NativeMethods.b3GetBodyInfo(pybullet, 0, ref info);

                text.text = info.m_baseName;
            }
        }
    }
Exemple #3
0
    //TODO deal with loading status
    public int LoadUrdfAndGetBodyIdx(string modelName, Vector3 p, Quaternion qq)
    {
        IntPtr cmd = NativeMethods.b3LoadUrdfCommandInit(_native, modelName);

        //IntPtr cmd = NativeMethods.b3LoadMJCFCommandInit(_native, modelName);

        NativeMethods.b3LoadUrdfCommandSetStartOrientation(cmd, qq.x, qq.y, qq.z, qq.w);
        NativeMethods.b3LoadUrdfCommandSetStartPosition(cmd, p.x, p.y, p.z);
        IntPtr status  = NativeMethods.b3SubmitClientCommandAndWaitStatus(_native, cmd);
        int    bodyIdx = NativeMethods.b3GetStatusBodyIndex(status);

        Debug.Log("body index " + bodyIdx);
        cmd    = NativeMethods.b3InitRequestVisualShapeInformation(_native, bodyIdx);
        status = NativeMethods.b3SubmitClientCommandAndWaitStatus(_native, cmd);
        EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);

        if (statusType == EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_INFO_COMPLETED ||
            statusType == EnumSharedMemoryServerStatus.CMD_VISUAL_SHAPE_UPDATE_COMPLETED)
        {
            b3VisualShapeInformation visuals = new b3VisualShapeInformation();
            NativeMethods.b3GetVisualShapeInformation(_native, ref visuals);
            Debug.Log("visuals.m_numVisualShapes=" + visuals.m_numVisualShapes);
            int           numVisualShapes = visuals.m_numVisualShapes;
            System.IntPtr visualPtr       = visuals.m_visualShapeData;

            //robot.bodies = new Body[visuals.m_numVisualShapes];

            for (int s = 0; s < visuals.m_numVisualShapes; s++)
            {
                b3VisualShapeData visual = (b3VisualShapeData)Marshal.PtrToStructure(visualPtr, typeof(b3VisualShapeData));
                visualPtr = new IntPtr(visualPtr.ToInt64() + (Marshal.SizeOf(typeof(b3VisualShapeData))));
                Vector3 scale;
                scale.x = (float)visual.m_dimensions[0];
                scale.y = (float)visual.m_dimensions[1];
                scale.z = (float)visual.m_dimensions[2];

                Debug.Log("visual.m_visualGeometryType =" + (eUrdfGeomTypes)visual.m_visualGeometryType +
                          " linkIndex=" + visual.m_linkIndex + " id=" + visual.m_objectUniqueId);

                Vector3 pos;
                pos.x = (float)visual.m_localVisualFrame[0];
                pos.y = (float)visual.m_localVisualFrame[1];
                pos.z = (float)visual.m_localVisualFrame[2];
                Quaternion rot;
                rot.x = (float)visual.m_localVisualFrame[3];
                rot.y = (float)visual.m_localVisualFrame[4];
                rot.z = (float)visual.m_localVisualFrame[5];
                rot.w = (float)visual.m_localVisualFrame[6];

                //robot.bodies[s] = CreateShape(visual, "cube", pos, rot, scale);

                if (visual.m_visualGeometryType == (int)eUrdfGeomTypes.GEOM_MESH)
                {
                    Debug.Log("visual.m_meshAssetFileName=" + visual.m_meshAssetFileName);
                }
            }
        }

        int numJoints = NativeMethods.b3GetNumJoints(_native, bodyIdx);

        Dictionary <string, int> jointNameToId = new Dictionary <string, int>();

        for (int i = 0; i < numJoints; i++)
        {
            b3JointInfo jointInfo = new b3JointInfo();
            NativeMethods.b3GetJointInfo(_native, bodyIdx, i, ref jointInfo);
            jointNameToId.Add(jointInfo.m_jointName, i);
            Debug.Log("Joint name " + jointInfo.m_jointName + " Link name " + jointInfo.m_linkName);
        }

        //NativeMethods.collisionshape(_native, bodyIdx);

        return(bodyIdx);
    }