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

make setJointPosMultiDof and setJointVelMultiDof argument const.

Browse Source 
add PyBullet.resetJointStateMultiDof / getJointStateMultiDof, for preliminary support for spherical and planar joints
This commit is contained in:
erwincoumans
2018-11-10 14:26:31 -08:00
parent 642c6a71d2
commit 17219f84c6
14 changed files with 512 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -972,6 +972,51 @@ B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemo
return 0;
}
B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState2* state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status);
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
b3Assert(bodyIndex >= 0);
if (bodyIndex >= 0)
{
state->m_qDofSize = 0;
state->m_uDofSize = 0;
b3JointInfo 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))
{
state->m_qDofSize = info.m_qSize;
state->m_uDofSize = info.m_uSize;
for (int i = 0; i < state->m_qDofSize; i++)
{
state->m_jointPosition[i] = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex + i];
}
for (int i = 0; i < state->m_uDofSize; i++)
{
state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex+i];
}
}
else
{
state->m_jointPosition[0] = 0;
state->m_jointVelocity[0] = 0;
}
for (int ii(0); ii < 6; ++ii)
{
state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1;
}
}
return 0;
}
B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState* state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
@@ -1812,6 +1857,72 @@ B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle phys
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetJointPositionMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointPosition, int posSize)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_JOINT_STATE;
b3JointInfo info;
b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId, jointIndex, &info);
//if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >= 0)
if (info.m_qIndex >= 0)
{
if (posSize == info.m_qSize)
{
for (int i = 0; i < posSize; i++)
{
command->m_initPoseArgs.m_initialStateQ[info.m_qIndex + i] = jointPosition[i];
command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex + i] = 1;
}
}
}
return 0;
}
B3_SHARED_API 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;
}
B3_SHARED_API int b3CreatePoseCommandSetJointVelocityMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointVelocity, int velSize)
{
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);
//if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >= 0) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
if ((info.m_uIndex >= 0) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
{
if (velSize == info.m_uSize)
{
for (int i = 0; i < velSize; i++)
{
command->m_initPoseArgs.m_initialStateQdot[info.m_uIndex + i] = jointVelocity[i];
command->m_initPoseArgs.m_hasInitialStateQdot[info.m_uIndex + i] = 1;
}
}
}
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -1848,22 +1959,7 @@ B3_SHARED_API int b3CreatePoseCommandSetQdots(b3SharedMemoryCommandHandle comman
return 0;
}
B3_SHARED_API 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;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -524,13 +524,15 @@ extern "C"
B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
B3_SHARED_API int b3CreatePoseCommandSetJointPositionMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointPosition, int posSize);
B3_SHARED_API int b3CreatePoseCommandSetQ(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* q, const int* hasQ);
B3_SHARED_API int b3CreatePoseCommandSetQdots(b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* qDots, const int* hasQdots);
B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities);
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity);
B3_SHARED_API int b3CreatePoseCommandSetJointVelocityMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointVelocity, int velSize);
///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.
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
@@ -546,6 +548,7 @@ extern "C"
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 b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState2* state);
B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState* state);
B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,

26
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -149,6 +149,32 @@ bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b
if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size()))
{
info = bodyJoints->m_jointInfo[jointIndex];
switch (info.m_jointType)
{
case eSphericalType:
{
info.m_qSize = 4;//quaterion x,y,z,w
info.m_uSize = 3;
break;
}
case ePlanarType:
{
info.m_qSize = 2;
info.m_uSize = 2;
break;
}
case ePrismaticType:
case eRevoluteType:
{
info.m_qSize = 1;
info.m_uSize = 1;
break;
}
default:
{
}
}
return true;
}
}

31
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -1291,6 +1291,37 @@ bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointIn
if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size()))
{
info = bodyJoints->m_jointInfo[jointIndex];
info.m_qSize = 0;
info.m_uSize = 0;
switch (info.m_jointType)
{
case eSphericalType:
{
info.m_qSize = 4;//quaterion x,y,z,w
info.m_uSize = 3;
break;
}
case ePlanarType:
{
info.m_qSize = 2;
info.m_uSize = 2;
break;
}
case ePrismaticType:
case eRevoluteType:
{
info.m_qSize = 1;
info.m_uSize = 1;
break;
}
default:
{
}
}
return true;
}
}

34
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -8007,17 +8007,39 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
int posVarCountIndex = 7;
for (int i = 0; i < mb->getNumLinks(); i++)
{
if ((clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex]) && (mb->getLink(i).m_dofCount == 1))
bool hasPosVar = true;
for (int j = 0; j < mb->getLink(i).m_posVarCount; j++)
{
mb->setJointPos(i, clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
mb->setJointVel(i, 0); //backwards compatibility
if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex+j] == 0)
{
hasPosVar = false;
break;
}
}
if ((clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex]) && (mb->getLink(i).m_dofCount == 1))
if (hasPosVar)
{
btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex];
mb->setJointVel(i, vel);
mb->setJointPosMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
double vel[6] = { 0, 0, 0, 0, 0, 0 };
mb->setJointVelMultiDof(i, vel);
}
bool hasVel = true;
for (int j = 0; j < mb->getLink(i).m_dofCount; j++)
{
if (clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex + j] == 0)
{
hasVel = false;
break;
}
}
if (hasVel)
{
mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]);
}
posVarCountIndex += mb->getLink(i).m_posVarCount;
uDofIndex += mb->getLink(i).m_dofCount;
}

12
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -274,6 +274,8 @@ struct b3JointInfo
double m_childFrame[7]; // ^^^
double m_jointAxis[3]; // joint axis in parent local frame
int m_parentIndex;
int m_qSize;
int m_uSize;
};
enum UserDataValueType
@@ -357,6 +359,16 @@ struct b3JointSensorState
double m_jointMotorTorque;
};
struct b3JointSensorState2
{
double m_jointPosition[4];
double m_jointVelocity[3];
double m_jointReactionForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
double m_jointMotorTorque;
int m_qDofSize;
int m_uDofSize;
};
struct b3DebugLines
{
int m_numDebugLines;