From 17219f84c62e5fe3888b934a954a91b7774bd4b2 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 10 Nov 2018 14:26:31 -0800 Subject: [PATCH] make setJointPosMultiDof and setJointVelMultiDof argument const. add PyBullet.resetJointStateMultiDof / getJointStateMultiDof, for preliminary support for spherical and planar joints --- data/hinge.urdf | 9 +- .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 63 ++++- .../Importers/ImportURDFDemo/URDFJointTypes.h | 2 + .../Importers/ImportURDFDemo/UrdfParser.cpp | 4 +- examples/SharedMemory/PhysicsClientC_API.cpp | 128 ++++++++-- examples/SharedMemory/PhysicsClientC_API.h | 5 +- .../PhysicsClientSharedMemory.cpp | 26 ++ examples/SharedMemory/PhysicsDirect.cpp | 31 +++ .../PhysicsServerCommandProcessor.cpp | 34 ++- examples/SharedMemory/SharedMemoryPublic.h | 12 + examples/pybullet/examples/constraint.py | 4 +- examples/pybullet/pybullet.c | 222 ++++++++++++++++++ .../Featherstone/btMultiBody.cpp | 4 +- src/BulletDynamics/Featherstone/btMultiBody.h | 4 +- 14 files changed, 512 insertions(+), 36 deletions(-) diff --git a/data/hinge.urdf b/data/hinge.urdf index 080fed40a..87d5a0e6d 100644 --- a/data/hinge.urdf +++ b/data/hinge.urdf @@ -38,7 +38,7 @@ - + @@ -54,20 +54,21 @@ - + - + - + + diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 9930ac994..59a961772 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -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); diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index f35f68bda..67e5affca 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -11,6 +11,8 @@ enum UrdfJointTypes URDFFloatingJoint, URDFPlanarJoint, URDFFixedJoint, + URDFSphericalJoint, + }; enum URDF_LinkContactFlags diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 162d37f4c..bc9936f14 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -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; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0f684bae6..834351f62 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index a556b9728..afceb7784 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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, diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 5846c16a0..c9193ecef 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -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; } } diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index de5d7e1de..7f6c2ec6a 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -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; } } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2e0fb0b94..90ef1f1de 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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; } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index eb74d3f3c..47ee7f531 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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; diff --git a/examples/pybullet/examples/constraint.py b/examples/pybullet/examples/constraint.py index 51ac0750a..5baf6952a 100644 --- a/examples/pybullet/examples/constraint.py +++ b/examples/pybullet/examples/constraint.py @@ -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: diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 375b41a0f..777a011a9 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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."}, diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 172472988..bd8cc6511 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -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]; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 8a693f539..e8b78adf7 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -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.