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

bump up shared memory version number

Browse Source 
add option to recompute forward kinematics, to be consistent with link velocities in pybullet.getLinkState (..., computeForwardKinematics=0/1), thanks to Jeff Bingham for bringing up this inconsistency
This commit is contained in:
erwincoumans
2017-09-26 10:05:17 -07:00
parent 270a036cd7
commit b1f8eb74a4
7 changed files with 71 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -649,6 +649,18 @@ B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryC
return 0;
}
B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE);
if (computeForwardKinematics && command->m_type == CMD_REQUEST_ACTUAL_STATE)
{
command->m_updateFlags |= ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS;
}
return 0;
}
B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
{

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -439,6 +439,8 @@ B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle com
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity);
B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics);
B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);

9
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -1204,6 +1204,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
break;
}
case CMD_CALCULATED_JACOBIAN_COMPLETED:
{
break;
}
case CMD_CALCULATED_JACOBIAN_FAILED:
{
b3Warning("jacobian calculation failed");
break;
}
case CMD_CUSTOM_COMMAND_FAILED:
{
b3Warning("custom plugin command failed");

26
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -949,7 +949,31 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
b3Warning("custom plugin command failed");
break;
}
case CMD_CLIENT_COMMAND_COMPLETED:
{
break;
}
case CMD_CALCULATED_JACOBIAN_COMPLETED:
{
break;
}
case CMD_CALCULATED_JACOBIAN_FAILED:
{
b3Warning("jacobian calculation failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
break;
}
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{
break;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
break;
}
default:
{
//b3Warning("Unknown server status type");

11
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -5205,6 +5205,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<btVector3> omega;
btAlignedObjectArray<btVector3> linVel;
bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0);
if (computeForwardKinematics)
{
B3_PROFILE("compForwardKinematics");
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
world_to_local.resize(mb->getNumLinks()+1);
local_origin.resize(mb->getNumLinks()+1);
mb->forwardKinematics(world_to_local,local_origin);
}
bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0);
if (computeLinkVelocities)
{

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,8 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201708270
#define SHARED_MEMORY_MAGIC_NUMBER 201709260
//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
//#define SHARED_MEMORY_MAGIC_NUMBER 201707140
//#define SHARED_MEMORY_MAGIC_NUMBER 201706015
//#define SHARED_MEMORY_MAGIC_NUMBER 201706001
@@ -505,7 +506,8 @@ struct b3VisualShapeInformation
enum eLinkStateFlags
{
ACTUAL_STATE_COMPUTE_LINKVELOCITY=1
ACTUAL_STATE_COMPUTE_LINKVELOCITY=1,
ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS=2,
};
///b3LinkState provides extra information such as the Cartesian world coordinates

10
examples/pybullet/pybullet.c
View File

@@ -3126,13 +3126,14 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject*
int bodyUniqueId = -1;
int linkIndex = -1;
int computeLinkVelocity = 0;
int computeForwardKinematics = 0;
int i;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "computeLinkVelocity", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex,&computeLinkVelocity, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "computeLinkVelocity", "computeForwardKinematics", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|iii", kwlist, &bodyUniqueId, &linkIndex,&computeLinkVelocity,&computeForwardKinematics,&physicsClientId))
{
return NULL;
}
@@ -3168,6 +3169,11 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject*
b3RequestActualStateCommandComputeLinkVelocity(cmd_handle,computeLinkVelocity);
}
if (computeForwardKinematics)
{
b3RequestActualStateCommandComputeForwardKinematics(cmd_handle,computeForwardKinematics);
}
status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);