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

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2017-03-23 13:55:44 -07:00
parent d717b1db84 1aff189fdd
commit 0a552c3ba1
5 changed files with 67 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

44
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -572,7 +572,7 @@ int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
if (bodyIndex>=0)
{
b3JointInfo info;
bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info)!=0;
if (result)
{
if ((info.m_qIndex>=0) && (info.m_uIndex>=0) && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
@@ -746,6 +746,7 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
command->m_initPoseArgs.m_hasInitialStateQdot[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
}
@@ -828,8 +829,11 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE;
for (int i=0;i<numJointPositions;i++)
{
command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
if ((i+7)<MAX_DEGREE_OF_FREEDOM)
{
command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
}
}
return 0;
}
@@ -853,6 +857,40 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
return 0;
}
int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |=INIT_POSE_HAS_JOINT_VELOCITY;
for (int i=0;i<numJointVelocities;i++)
{
if ((i+6)<MAX_DEGREE_OF_FREEDOM)
{
command->m_initPoseArgs.m_initialStateQdot[i+6] = jointVelocities[i];
command->m_initPoseArgs.m_hasInitialStateQdot[i+6] = 1;
}
}
return 0;
}
int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |=INIT_POSE_HAS_JOINT_VELOCITY;
b3JointInfo info;
b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info);
btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0);
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >=0) && (info.m_uIndex<MAX_DEGREE_OF_FREEDOM))
{
command->m_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity;
command->m_initPoseArgs.m_hasInitialStateQdot[info.m_uIndex] = 1;
}
return 0;
}

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -304,6 +304,9 @@ int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle comman
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities);
int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity);
///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
///This is rather inconsistent, to mix programmatical creation with loading from file.
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);

19
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3365,15 +3365,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
{
int dofIndex = 7;
int uDofIndex = 6;
int posVarCountIndex = 7;
for (int i=0;i<mb->getNumLinks();i++)
{
if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[dofIndex]) && (mb->getLink(i).m_dofCount==1))
if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex]) && (mb->getLink(i).m_dofCount==1))
{
mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]);
mb->setJointVel(i,0);
mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
mb->setJointVel(i,0);//backwards compatibility
}
dofIndex += mb->getLink(i).m_dofCount;
if ((clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex]) && (mb->getLink(i).m_dofCount==1))
{
btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex];
mb->setJointVel(i,vel);
}
posVarCountIndex += mb->getLink(i).m_posVarCount;
uDofIndex += mb->getLink(i).m_dofCount;
}
}

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -117,6 +117,7 @@ enum EnumInitPoseFlags
INIT_POSE_HAS_JOINT_STATE=4,
INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8,
INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16,
INIT_POSE_HAS_JOINT_VELOCITY=32,
};