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

9
data/hinge.urdf
View File

@@ -38,7 +38,7 @@
</geometry>
</collision>
</link>
<joint name="joint_baseLink_childA" type="continuous">
<joint name="joint_baseLink_childA" type="spherical">
<parent link="baseLink"/>
<child link="childA"/>
<origin xyz="0 0 0"/>
@@ -54,20 +54,21 @@
<visual>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<geometry>
<sphere radius="0.2"/>
<box size="0.1 0.2 0.3 "/>
</geometry>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<geometry>
<sphere radius="0.2"/>
<box size="0.1 0.2 0.3 "/>
</geometry>
</collision>
</link>
<joint name="joint_childA_childB" type="fixed">
<joint name="joint_childA_childB" type="continuous">
<parent link="childA"/>
<child link="childB"/>
<origin xyz="0 0 -0.2"/>
<axis xyz="1 0 0"/>
</joint>
</robot>

63
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -383,8 +383,65 @@ void ConvertURDF2BulletInternal(
switch (jointType)
{
case URDFFloatingJoint:
case URDFSphericalJoint:
{
if (createMultiBody)
{
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
cache.m_bulletMultiBody->setupSpherical(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin(),
disableParentCollision);
}
else
{
btAssert(0);
}
break;
}
case URDFPlanarJoint:
{
if (createMultiBody)
{
#if 0
void setupPlanar(int i, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &rotationAxis,
const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
bool disableParentCollision = false);
#endif
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
cache.m_bulletMultiBody->setupPlanar(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(),
disableParentCollision);
}
else
{
#if 0
//b3Printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = 0;
//backward compatibility
if (flags & CUF_RESERVED)
{
dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
}
else
{
dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
}
if (enableConstraints)
world1->addConstraint(dof6, true);
#endif
}
break;
}
case URDFFloatingJoint:
case URDFFixedJoint:
{
if ((jointType == URDFFloatingJoint) || (jointType == URDFPlanarJoint))
@@ -426,7 +483,9 @@ void ConvertURDF2BulletInternal(
if (createMultiBody)
{
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(),
parentRotToThis, quatRotate(offsetInB.getRotation(),
jointAxisInJointSpace),
offsetInA.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);

2
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@@ -11,6 +11,8 @@ enum UrdfJointTypes
URDFFloatingJoint,
URDFPlanarJoint,
URDFFixedJoint,
URDFSphericalJoint,
};
enum URDF_LinkContactFlags

4
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -1280,7 +1280,9 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, XMLElement* config, ErrorLogger* l
}
std::string type_str = type_char;
if (type_str == "planar")
if (type_str == "spherical")
joint.m_type = URDFSphericalJoint;
else if (type_str == "planar")
joint.m_type = URDFPlanarJoint;
else if (type_str == "floating")
joint.m_type = URDFFloatingJoint;

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;

4
examples/pybullet/examples/constraint.py
View File

@@ -9,8 +9,8 @@ cubeId = p.loadURDF("cube_small.urdf",0,0,1)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
print cid
print p.getConstraintUniqueId(0)
print (cid)
print (p.getConstraintUniqueId(0))
prev=[0,0,1]
a=-math.pi
while 1:

222
examples/pybullet/pybullet.c
View File

@@ -3450,6 +3450,112 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObje
return Py_None;
}
// Initalize all joint positions given a list of values
static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args, PyObject* keywds)
{
{
int bodyUniqueId;
int jointIndex;
double targetPositionArray[4] = { 0, 0, 0, 0 };
double targetVelocityArray[3] = { 0, 0, 0 };
int targetPositionSize = 0;
int targetVelocitySize = 0;
b3PhysicsClientHandle sm = 0;
PyObject* targetPositionObj = 0;
PyObject* targetVelocityObj = 0;
int physicsClientId = 0;
static char* kwlist[] = { "bodyUniqueId", "jointIndex", "targetValue", "targetVelocity", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|Oi", kwlist, &bodyUniqueId, &jointIndex, &targetPositionObj, &targetVelocityObj, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (targetPositionObj)
{
PyObject* targetPositionSeq = 0;
int i = 0;
targetPositionSeq = PySequence_Fast(targetPositionObj, "expected a targetPosition sequence");
targetPositionSize = PySequence_Size(targetPositionObj);
if (targetPositionSize < 0)
{
targetPositionSize = 0;
}
if (targetPositionSize >4)
{
targetPositionSize = 4;
}
for (i = 0; i < targetPositionSize; i++)
{
targetPositionArray[i] = pybullet_internalGetFloatFromSequence(targetPositionSeq, i);
}
}
if (targetVelocityObj)
{
int i = 0;
PyObject* targetVelocitySeq = 0;
targetVelocitySeq = PySequence_Fast(targetVelocityObj, "expected a targetVelocity sequence");
targetVelocitySize = PySequence_Size(targetVelocityObj);
if (targetVelocitySize < 0)
{
targetVelocitySize = 0;
}
if (targetVelocitySize >3)
{
targetVelocitySize = 3;
}
for (i = 0; i < targetVelocitySize; i++)
{
targetVelocityArray[i] = pybullet_internalGetFloatFromSequence(targetVelocitySeq, i);
}
}
if (targetPositionSize == 0 && targetVelocitySize == 0)
{
PyErr_SetString(SpamError, "Expected an position and/or velocity list.");
return NULL;
}
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int numJoints;
numJoints = b3GetNumJoints(sm, bodyUniqueId);
if ((jointIndex >= numJoints) || (jointIndex < 0))
{
PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL;
}
commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
if (targetPositionSize)
{
b3CreatePoseCommandSetJointPositionMultiDof(sm, commandHandle, jointIndex, targetPositionArray, targetPositionSize);
}
if (targetVelocitySize)
{
b3CreatePoseCommandSetJointVelocityMultiDof(sm, commandHandle, jointIndex, targetVelocityArray, targetVelocitySize);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject* keywds)
{
static char* kwlist[] = {"objectUniqueId", "linearVelocity", "angularVelocity", "physicsClientId", NULL};
@@ -3831,6 +3937,114 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args, PyObject
return Py_None;
}
static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* pyListJointForceTorque;
PyObject* pyListJointState;
PyObject* pyListPosition;
PyObject* pyListVelocity;
struct b3JointSensorState2 sensorState;
int bodyUniqueId = -1;
int jointIndex = -1;
int sensorStateSize = 4; // size of struct b3JointSensorState
int forceTorqueSize = 6; // size of force torque list from b3JointSensorState
int j;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = { "bodyUniqueId", "jointIndex", "physicsClientId", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &jointIndex, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
{
int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle;
b3SharedMemoryStatusHandle status_handle;
if (bodyUniqueId < 0)
{
PyErr_SetString(SpamError, "getJointState failed; invalid bodyUniqueId");
return NULL;
}
if (jointIndex < 0)
{
PyErr_SetString(SpamError, "getJointState failed; invalid jointIndex");
return NULL;
}
cmd_handle =
b3RequestActualStateCommandInit(sm, bodyUniqueId);
status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
status_type = b3GetStatusType(status_handle);
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getJointState failed.");
return NULL;
}
pyListJointState = PyTuple_New(sensorStateSize);
pyListJointForceTorque = PyTuple_New(forceTorqueSize);
if (b3GetJointStateMultiDof(sm, status_handle, jointIndex, &sensorState))
{
int i = 0;
pyListPosition = PyTuple_New(sensorState.m_qDofSize);
pyListVelocity = PyTuple_New(sensorState.m_uDofSize);
PyTuple_SetItem(pyListJointState, 0, pyListPosition);
PyTuple_SetItem(pyListJointState, 1, pyListVelocity);
for (i = 0; i < sensorState.m_qDofSize; i++)
{
PyTuple_SetItem(pyListPosition, i,
PyFloat_FromDouble(sensorState.m_jointPosition[i]));
}
for (i = 0; i < sensorState.m_uDofSize; i++)
{
PyTuple_SetItem(pyListVelocity, i,
PyFloat_FromDouble(sensorState.m_jointVelocity[i]));
}
for (j = 0; j < forceTorqueSize; j++)
{
PyTuple_SetItem(pyListJointForceTorque, j,
PyFloat_FromDouble(sensorState.m_jointReactionForceTorque[j]));
}
PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
PyTuple_SetItem(pyListJointState, 3,
PyFloat_FromDouble(sensorState.m_jointMotorTorque));
return pyListJointState;
}
else
{
PyErr_SetString(SpamError, "getJointState failed (2).");
return NULL;
}
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getJointStates(PyObject* self, PyObject* args, PyObject* keywds)
{
PyObject* pyListJointForceTorque;
@@ -9494,6 +9708,9 @@ static PyMethodDef SpamMethods[] = {
{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for a joint on a body."},
{ "getJointStateMultiDof", (PyCFunction)pybullet_getJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for a joint on a body. (supports planar and spherical joints)" },
{"getJointStates", (PyCFunction)pybullet_getJointStates, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for multiple joints on a body."},
@@ -9513,6 +9730,11 @@ static PyMethodDef SpamMethods[] = {
"Reset the state (position, velocity etc) for a joint on a body "
"instantaneously, not through physics simulation."},
{ "resetJointStateMultiDof", (PyCFunction)pybullet_resetJointStateMultiDof, METH_VARARGS | METH_KEYWORDS,
"resetJointStateMultiDof(objectUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0)\n"
"Reset the state (position, velocity etc) for a joint on a body "
"instantaneously, not through physics simulation." },
{"changeDynamics", (PyCFunction)pybullet_changeDynamicsInfo, METH_VARARGS | METH_KEYWORDS,
"change dynamics information such as mass, lateral friction coefficient."},

4
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -396,7 +396,7 @@ void btMultiBody::setJointPos(int i, btScalar q)
m_links[i].updateCacheMultiDof();
}
void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
void btMultiBody::setJointPosMultiDof(int i, const btScalar *q)
{
for (int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
m_links[i].m_jointPos[pos] = q[pos];
@@ -409,7 +409,7 @@ void btMultiBody::setJointVel(int i, btScalar qdot)
m_realBuf[6 + m_links[i].m_dofOffset] = qdot;
}
void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
void btMultiBody::setJointVelMultiDof(int i, const btScalar *qdot)
{
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];

4
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -235,8 +235,8 @@ public:
void setJointPos(int i, btScalar q);
void setJointVel(int i, btScalar qdot);
void setJointPosMultiDof(int i, btScalar *q);
void setJointVelMultiDof(int i, btScalar *qdot);
void setJointPosMultiDof(int i, const btScalar *q);
void setJointVelMultiDof(int i, const btScalar *qdot);
//
// direct access to velocities as a vector of 6 + num_links elements.