make setJointPosMultiDof and setJointVelMultiDof argument const.

add PyBullet.resetJointStateMultiDof / getJointStateMultiDof, for preliminary support for spherical and planar joints

data/hinge.urdf

@@ -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>
 

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);
 

examples/Importers/ImportURDFDemo/URDFJointTypes.h

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

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;

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)
 {

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,

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;
 		}
 	}

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;
 		}
 	}

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]);
+					if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex+j] == 0)
-					mb->setJointVel(i, 0);  //backwards compatibility
+					{
+						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->setJointPosMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
-					mb->setJointVel(i, vel);
+					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;
 			}

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;

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 (cid)
-print p.getConstraintUniqueId(0)
+print (p.getConstraintUniqueId(0))
 prev=[0,0,1]
 a=-math.pi
 while 1:

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."},
 

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];

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 setJointPosMultiDof(int i, const btScalar *q);
-	void setJointVelMultiDof(int i, btScalar *qdot);
+	void setJointVelMultiDof(int i, const btScalar *qdot);
 
 	//
 	// direct access to velocities as a vector of 6 + num_links elements.