public Body CreateShape(b3VisualShapeData shapeData, string prefabName, Vector3 p, Quaternion qq, Vector3 scale) { GameObject prefab = null; for (int i = 0; i < PrimitivePrefabs.Length; i++) { if (PrimitivePrefabs[i].name == prefabName) { prefab = PrimitivePrefabs[i]; } } if (prefab == null) { Debug.LogError("Could not find a prefab named " + prefabName); return(null); } Body bdy = new Body(); bdy.unityProxy = GameObject.Instantiate <GameObject>(prefab).transform; bdy.linkIndex = shapeData.m_linkIndex; bdy.objectID = shapeData.m_objectUniqueId; bdy.unityProxy.position = p; bdy.unityProxy.rotation = qq; bdy.unityProxy.localScale = scale; return(bdy); }
// 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; } } }
// 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); } } }
//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); }