C API, how to get body position/orientation by id (URDF)

Post Reply
User avatar
harha
Posts: 1
Joined: Tue Jan 08, 2019 4:15 am

C API, how to get body position/orientation by id (URDF)

Post by harha » Tue Jan 08, 2019 4:22 am

Hey.

I integrated bullet3 to my game prototype which is written in C. I'm trying to load URDF/OBJ for my simple vehicle object, currently it's just a box with mass, etc. It gets loaded fine and I can step the simulation, now I'd like to link the position and orientation of such object with my game engine's representation of a vehicle.

So I just have this simple vehicle_update method which takes a pointer to a vehicle and pointer to the physics client object. I'd like to query the input vehicle physics info there and update my own transform object vec4 position and quaternion orientations, so that I can see how the simulation behaves in my game.

I've been looking all around at the C api header file + examples but I cannot find a method that does this for me. I can get b3BodyInfo and b3DynamicsInfo structs populated with correct data by using the unique physics id of the vehicle that was generated when I loaded the URDF file. But those don't seem to provide any info about current position and orientation of the shapes.

So is there a way to do this with the C api? I'd certainly think so? If there isn't, I guess I have to extend the API myself by adding few functions...

Here's how I init bullet3: (gs->world->phys_cli == b3PhysicsClientHandle)

Code: Select all

// init bullet physics engine
gs->world->phys_cli = b3ConnectPhysicsDirect();

if (b3CanSubmitCommand(gs->world->phys_cli))
{
	// reset simulation
	b3SharedMemoryStatusHandle phys_status_reset = b3SubmitClientCommandAndWaitStatus(gs->world->phys_cli, b3InitResetSimulationCommand(gs->world->phys_cli));

	// set world gravity
	b3SharedMemoryCommandHandle phys_cmd_gravity = b3InitPhysicsParamCommand(gs->world->phys_cli);
	b3PhysicsParamSetGravity(phys_cmd_gravity, 0.0f, -9.80665f, 0.0f);
	b3SharedMemoryStatusHandle phy_status_gravity = b3SubmitClientCommandAndWaitStatus(gs->world->phys_cli, phys_cmd_gravity);
}
else
{
	printf("game.c: Error, cannot connect to direct bullet3 physics server!\n");
}
Here's how I init a vehicle on bullet3 side:

Code: Select all

// init vehicle physics body
b3SharedMemoryCommandHandle phys_cmd_urdf = b3LoadUrdfCommandInit(phys_cli, fp_model_urdf);
b3LoadUrdfCommandSetStartPosition(phys_cmd_urdf, vehicle->trf.pos.x, vehicle->trf.pos.y, vehicle->trf.pos.z);
b3LoadUrdfCommandSetStartOrientation(phys_cmd_urdf, vehicle->trf.ori.x, vehicle->trf.ori.y, vehicle->trf.ori.z, vehicle->trf.ori.w);

b3SharedMemoryStatusHandle phys_status_urdf = b3SubmitClientCommandAndWaitStatus(phys_cli, phys_cmd_urdf);
int phys_status_urdf_type = b3GetStatusType(phys_status_urdf);

if (phys_status_urdf_type == CMD_URDF_LOADING_COMPLETED)
{
	vehicle->phys_body_id = b3GetStatusBodyIndex(phys_status_urdf);
}
else
{
	printf("vehicle.c: Cannot load URDF file %s! phys_status_urdf_type: %d.\n", fp_model_urdf, phys_status_urdf_type);
}
If I try to query for b3VisualShapeInformation or b3CollisionShapeInformation I get a result that says that the number of bodies is 0. If I call b3GetNumBodies(phys_client) I get a result that says the number of bodies is 1, which is correct.

I think I want to fetch m_visualShapeData[0].m_localVisualFrame for each body in the simulation, correct? Then how do I add my URDF's to the world because this m_visualShapeData array length is 0 if I fetch it, as I said (struct b3VisualShapeInformation).

Help appreciated!

edit:

I'm trying b3RequestActualStateCommandInit which seems to be what I want. But I don't get data if I call b3GetStatusActualState and try to read actualStateQ. The double array indices remain uninitialized...

edit2: I got it to work!

Code: Select all

// request physics body actual state
b3SharedMemoryCommandHandle phys_cmd_state = b3RequestActualStateCommandInit(phys_cli, vehicle->phys_body_id);

b3SharedMemoryStatusHandle phys_status_state = b3SubmitClientCommandAndWaitStatus(phys_cli, phys_cmd_state);
int phys_status_state_type = b3GetStatusType(phys_status_state);

// update vehicle position & orientation
if (phys_status_state_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
	const double * stateQ = NULL;

	int state = b3GetStatusActualState(
		phys_status_state,
		NULL,
		NULL,
		NULL,
		NULL,
		&stateQ,
		NULL,
		NULL
	);

	if (state)
	{
		vehicle->trf.pos.x = stateQ[0];
		vehicle->trf.pos.y = stateQ[1];
		vehicle->trf.pos.z = stateQ[2];
		vehicle->trf.ori.w = stateQ[3];
		vehicle->trf.ori.x = stateQ[4];
		vehicle->trf.ori.y = stateQ[5];
		vehicle->trf.ori.z = stateQ[6];
		vehicle->trf.state_changed = 1;
	}
}
else
{
	printf("vehicle.c: Unable to physics body actual state! phys_status_state_type: %d.\n", phys_status_state_type);
}

mnaveau
Posts: 4
Joined: Tue Jan 15, 2019 4:11 pm

Re: C API, how to get body position/orientation by id (URDF)

Post by mnaveau » Tue Jan 15, 2019 5:01 pm

Hi,

I am trying to use the C api (without the shared memory). Though I cannot seem to be able to use the URDF parser with a simple "sudo apt-get install libbullet*"
How did you get to use the urdf parser from C?

User avatar
Erwin Coumans
Site Admin
Posts: 4155
Joined: Sun Jun 26, 2005 6:43 pm
Location: California, USA
Contact:

Re: C API, how to get body position/orientation by id (URDF)

Post by Erwin Coumans » Tue Jan 15, 2019 5:40 pm

It is easiest to look into the PyBullet documentation and how it uses the C API in pybullet.c or use/enhance the BulletRobotics C++ wrapper on top of the C API.

@mnaveau, this is example code, so you are supposed to copy it into your own project, if you want to use it. See also BulletRobotics target under Bullet/Extras for cmake/premake files.

Post Reply