Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

use explicit size for name, to avoid issue converting/marshalling data to C#

Browse Source 
example to call b3VisualShapeInformation from C# and marshall b3VisualShapeData (after loadURDF)
See examples\pybullet\unity3d\examples\NewBehaviourScript.cs
This commit is contained in:
erwincoumans
2017-10-03 18:01:53 -07:00
parent 9577875fe9
commit 1b03871b4d
7 changed files with 79 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/SharedMemory/BodyJointInfoUtility.h
View File

@@ -34,8 +34,8 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
{
{
b3JointInfo info;
info.m_jointName = 0;
info.m_linkName = 0;
info.m_jointName[0] = 0;
info.m_linkName[0] = 0;
info.m_flags = 0;
info.m_jointIndex = link;
info.m_qIndex =
@@ -48,14 +48,16 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
b3Printf("mb->m_links[%d].m_linkName = %s\n", link,
mb->m_links[link].m_linkName);
}
info.m_linkName = strDup(mb->m_links[link].m_linkName);
strcpy(info.m_linkName,mb->m_links[link].m_linkName);
}
if (mb->m_links[link].m_jointName) {
if (verboseOutput) {
b3Printf("mb->m_links[%d].m_jointName = %s\n", link,
mb->m_links[link].m_jointName);
}
info.m_jointName = strDup(mb->m_links[link].m_jointName);
strcpy(info.m_jointName,mb->m_links[link].m_jointName);
//info.m_jointName = strDup(mb->m_links[link].m_jointName);
}
info.m_jointType = mb->m_links[link].m_jointType;

5
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1438,7 +1438,10 @@ B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle com
B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
cl->disconnectSharedMemory();
if (cl)
{
cl->disconnectSharedMemory();
}
delete cl;
}

21
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -108,8 +108,8 @@ bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo&
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info.m_baseName = bodyJoints->m_baseName.c_str();
info.m_bodyName = bodyJoints->m_bodyName.c_str();
strcpy(info.m_baseName,bodyJoints->m_baseName.c_str());
strcpy(info.m_bodyName,bodyJoints->m_bodyName.c_str());
return true;
}
@@ -234,16 +234,6 @@ void PhysicsClientSharedMemory::resetData()
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
@@ -392,8 +382,8 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
template <typename T, typename U> void addJointInfoFromConstraint(int linkIndex, const T* con, U* bodyJoints, bool verboseOutput)
{
b3JointInfo info;
info.m_jointName = 0;
info.m_linkName = 0;
info.m_jointName[0] = 0;
info.m_linkName[0] = 0;
info.m_flags = 0;
info.m_jointIndex = linkIndex;
info.m_qIndex = linkIndex+7;
@@ -402,7 +392,8 @@ template <typename T, typename U> void addJointInfoFromConstraint(int linkIndex,
if (con->m_typeConstraintData.m_name)
{
info.m_jointName = strDup(con->m_typeConstraintData.m_name);
strcpy(info.m_jointName,con->m_typeConstraintData.m_name);
//info.m_linkName = strDup(con->m_typeConstraintData.m_name);
}

15
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -112,17 +112,6 @@ void PhysicsDirect::resetData()
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
for (int j = 0; j<bodyJoints->m_jointInfo.size(); j++) {
if (bodyJoints->m_jointInfo[j].m_jointName)
{
free(bodyJoints->m_jointInfo[j].m_jointName);
}
if (bodyJoints->m_jointInfo[j].m_linkName)
{
free(bodyJoints->m_jointInfo[j].m_linkName);
}
}
delete (*bodyJointsPtr);
}
}
@@ -1085,8 +1074,8 @@ bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
info.m_baseName = bodyJoints->m_baseName.c_str();
info.m_bodyName = bodyJoints->m_bodyName.c_str();
strcpy(info.m_baseName,bodyJoints->m_baseName.c_str());
strcpy(info.m_bodyName ,bodyJoints->m_bodyName .c_str());
return true;
}

8
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -214,8 +214,8 @@ enum b3JointInfoFlags
struct b3JointInfo
{
char* m_linkName;
char* m_jointName;
char m_linkName[1024];
char m_jointName[1024];
int m_jointType;
int m_qIndex;
int m_uIndex;
@@ -254,8 +254,8 @@ struct b3UserConstraint
struct b3BodyInfo
{
const char* m_baseName;
const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf
char m_baseName[1024];
char m_bodyName[1024]; // for btRigidBody, it does not have a base, but can still have a body name from urdf
};
struct b3DynamicsInfo

29
examples/pybullet/unity3d/autogen/NativeMethods.cs
View File

@@ -425,13 +425,13 @@ public enum b3JointInfoFlags {
[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)]
public struct b3JointInfo {
/// char*
[System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)]
/// char[1024]
[System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)]
public string m_linkName;
/// char*
[System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)]
/// char[1024]
[System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)]
public string m_jointName;
/// int
@@ -531,13 +531,13 @@ public struct b3UserConstraint {
[System.Runtime.InteropServices.StructLayoutAttribute(System.Runtime.InteropServices.LayoutKind.Sequential)]
public struct b3BodyInfo {
/// char*
[System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)]
/// char[1024]
[System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)]
public string m_baseName;
/// char*
[System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)]
/// char[1024]
[System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.ByValTStr, SizeConst = 1024)]
public string m_bodyName;
}
@@ -624,8 +624,7 @@ public struct b3CameraImageData {
public int m_pixelHeight;
/// char*
[System.Runtime.InteropServices.MarshalAsAttribute(System.Runtime.InteropServices.UnmanagedType.LPStr)]
public string m_rgbColorData;
public System.IntPtr m_rgbColorData;
/// float*
public System.IntPtr m_depthValues;
@@ -1444,7 +1443,7 @@ public static extern int b3GetBodyUniqueId(IntPtr physClient, int serialIndex)
///bodyUniqueId: int
///info: b3BodyInfo*
[System.Runtime.InteropServices.DllImportAttribute("pybullet_vs2010_x64_release.dll", EntryPoint="b3GetBodyInfo")]
public static extern int b3GetBodyInfo(IntPtr physClient, int bodyUniqueId, ref b3BodyInfo info) ;
public static extern int b3GetBodyInfo(IntPtr physClient, int bodyUniqueId, ref b3BodyInfo info) ;
/// Return Type: int

80
examples/pybullet/unity3d/examples/NewBehaviourScript.cs
View File

@@ -10,29 +10,7 @@ using System;
public class NewBehaviourScript : MonoBehaviour {
[DllImport("TestCppPlug.dll")]
static extern int Add(int a, int b);
[DllImport("TestCppPlug.dll")]
static extern int CallMe(Action<int> action);
[DllImport("TestCppPlug.dll")]
static extern IntPtr CreateSharedAPI(int id);
[DllImport("TestCppPlug.dll")]
static extern int GetMyIdPlusTen(IntPtr api);
[DllImport("TestCppPlug.dll")]
static extern void DeleteSharedAPI(IntPtr api);
private void IWasCalled(int value)
{
text.text = value.ToString();
}
Text text;
IntPtr sharedAPI;
IntPtr pybullet;
@@ -40,48 +18,76 @@ public class NewBehaviourScript : MonoBehaviour {
// Use this for initialization
void Start () {
text = GetComponent<Text>();
CallMe(IWasCalled);
sharedAPI = CreateSharedAPI(30);
pybullet = NativeMethods.b3ConnectPhysicsDirect();// SharedMemory(12347);
pybullet = NativeMethods.b3ConnectPhysicsDirect();// NativeMethods.b3ConnectSharedMemory(NativeConstants.SHARED_MEMORY_KEY);
IntPtr cmd = NativeMethods.b3InitResetSimulationCommand(pybullet);
IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
int numBodies = NativeMethods.b3GetNumBodies(pybullet);
cmd = NativeMethods.b3LoadUrdfCommandInit(pybullet, "plane.urdf");
status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
EnumSharedMemoryServerStatus statusType = (EnumSharedMemoryServerStatus)NativeMethods.b3GetStatusType(status);
if (statusType == EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED)
if (statusType == (EnumSharedMemoryServerStatus)EnumSharedMemoryServerStatus.CMD_URDF_LOADING_COMPLETED)
{
numBodies = NativeMethods.b3GetNumBodies(pybullet);
text.text = numBodies.ToString();
cmd = NativeMethods.b3InitRequestVisualShapeInformation(pybullet, 0);
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;
b3BodyInfo info=new b3BodyInfo();
NativeMethods.b3GetBodyInfo(pybullet, 0, ref info );
text.text = info.m_baseName;
}
}
//IntPtr clientPtr = b3CreateInProcessPhysicsServerAndConnect(0, ref ptr);
}
// Update is called once per frame
void Update () {
//if (pybullet != IntPtr.Zero)
if (pybullet != IntPtr.Zero)
{
IntPtr cmd = NativeMethods.b3InitStepSimulationCommand(pybullet);
IntPtr status = NativeMethods.b3SubmitClientCommandAndWaitStatus(pybullet, cmd);
//text.text = System.IO.Directory.GetCurrentDirectory().ToString();
//text.text = Add(4, 5).ToString();
//text.text = UnityEngine.Random.Range(0f, 1f).ToString();// GetMyIdPlusTen(sharedAPI).ToString();
}
}
void OnDestroy()
{
NativeMethods.b3DisconnectSharedMemory(pybullet);
if (pybullet != IntPtr.Zero)
{
NativeMethods.b3DisconnectSharedMemory(pybullet);
}
}
}