// 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);
}
}
}
// 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;
}
}
}
//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);
}