Implement faster array versions of PyBullet: getJointStatesMultiDof, getLinkStates, setJointMotorControlMultiDofArray, resetJointStatesMultiDof,

Implement StablePD in C++ through setJointMotorControlMultiDofArray method for pybullet_envs.deep_mimic, see testHumanoid.py and examples/pybullet/examples/humanoidMotionCapture.py
Minor fix in ChromeTraceUtil in case startTime>endTime (why would it happen?)

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -856,7 +856,17 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2Internal(b3S
 	for (int i = 0; i < MAX_DEGREE_OF_FREEDOM; i++)
 	{
 		command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
+
 	}
+	for (int i = 0; i < 7; i++)
+	{
+		command->m_sendDesiredStateCommandArgument.m_desiredStateQ[i] = 0;
+		command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
+		command->m_sendDesiredStateCommandArgument.m_Kp[i] = 0;
+		command->m_sendDesiredStateCommandArgument.m_Kd[i] = 0;
+		command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 0;
+	}
+	command->m_sendDesiredStateCommandArgument.m_desiredStateQ[3] = 1;
 	return (b3SharedMemoryCommandHandle)command;
 }
 

examples/SharedMemory/PhysicsClientC_API.h

@@ -472,6 +472,7 @@ extern "C"
 	///Only use when controlMode is CONTROL_MODE_VELOCITY
 	B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
 	B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount);
+	B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof2(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount);
 
 	B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
 	B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1788,32 +1788,11 @@ struct PhysicsServerCommandProcessorInternalData
 #ifdef STATIC_LINK_SPD_PLUGIN
 	b3HashMap<btHashPtr, cRBDModel*> m_rbdModels;
 
-	cRBDModel* findOrCreateRBDModel(btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot)
+	static void convertPose(const btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot, Eigen::VectorXd& pose, Eigen::VectorXd&vel)
 	{
-		cRBDModel* rbdModel = 0;
-		cRBDModel** rbdModelPtr = m_rbdModels.find(multiBody);
-		if (rbdModelPtr)
-		{
-			rbdModel = *rbdModelPtr;
-		}
-		else
-		{
-			rbdModel = new cRBDModel();
-			Eigen::MatrixXd bodyDefs;
-			Eigen::MatrixXd jointMat;
-			btExtractJointBodyFromBullet(multiBody, bodyDefs, jointMat);
-			btVector3 grav = m_dynamicsWorld->getGravity();
-			tVector3 gravity(grav[0], grav[1], grav[2], 0);
-			rbdModel->Init(jointMat, bodyDefs, gravity);
-			m_rbdModels.insert(multiBody, rbdModel);
-		}
-
-		//sync pose and vel
-
 		int baseDofQ = multiBody->hasFixedBase() ? 0 : 7;
 		int baseDofQdot = multiBody->hasFixedBase() ? 0 : 6;
 		
-		Eigen::VectorXd pose, vel;
 		pose.resize(7 + multiBody->getNumPosVars());
 		vel.resize(7 + multiBody->getNumPosVars());  //??
 
@@ -1910,11 +1889,40 @@ struct PhysicsServerCommandProcessorInternalData
 			}
 			}
 		}
+	}
+
+	cRBDModel* findOrCreateRBDModel(btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot)
+	{
+		cRBDModel* rbdModel = 0;
+		cRBDModel** rbdModelPtr = m_rbdModels.find(multiBody);
+		if (rbdModelPtr)
+		{
+			rbdModel = *rbdModelPtr;
+		}
+		else
+		{
+			rbdModel = new cRBDModel();
+			Eigen::MatrixXd bodyDefs;
+			Eigen::MatrixXd jointMat;
+			btExtractJointBodyFromBullet(multiBody, bodyDefs, jointMat);
+			btVector3 grav = m_dynamicsWorld->getGravity();
+			tVector3 gravity(grav[0], grav[1], grav[2], 0);
+			rbdModel->Init(jointMat, bodyDefs, gravity);
+			m_rbdModels.insert(multiBody, rbdModel);
+		}
+
+		//sync pose and vel
+
+		Eigen::VectorXd pose, vel;
+		PhysicsServerCommandProcessorInternalData::convertPose(multiBody, jointPositionsQ, jointVelocitiesQdot, pose, vel);
 
 		btVector3 gravOrg = m_dynamicsWorld->getGravity();
 		tVector grav(gravOrg[0], gravOrg[1], gravOrg[2], 0);
 		rbdModel->SetGravity(grav);
+		{
+			BT_PROFILE("rbdModel::Update");
 			rbdModel->Update(pose, vel);
+		}
 
 		return rbdModel;
 	}
@@ -6168,6 +6176,189 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 
 				break;
 			}
+#ifdef STATIC_LINK_SPD_PLUGIN
+			case CONTROL_MODE_STABLE_PD:
+			{
+				int posVal = body->m_multiBody->getNumPosVars();
+				btAlignedObjectArray<double> zeroVel;
+				int dof = 7 + posVal;
+				zeroVel.resize(dof);
+				//clientCmd.m_sendDesiredStateCommandArgument.
+				//current positions and velocities
+				btAlignedObjectArray<double> jointPositionsQ;
+				btAlignedObjectArray<double> jointVelocitiesQdot;
+				btTransform baseTr = body->m_multiBody->getBaseWorldTransform();
+#if 1
+				jointPositionsQ.push_back(baseTr.getOrigin()[0]);
+				jointPositionsQ.push_back(baseTr.getOrigin()[1]);
+				jointPositionsQ.push_back(baseTr.getOrigin()[2]);
+				jointPositionsQ.push_back(baseTr.getRotation()[0]);
+				jointPositionsQ.push_back(baseTr.getRotation()[1]);
+				jointPositionsQ.push_back(baseTr.getRotation()[2]);
+				jointPositionsQ.push_back(baseTr.getRotation()[3]);
+				jointVelocitiesQdot.push_back(body->m_multiBody->getBaseVel()[0]);
+				jointVelocitiesQdot.push_back(body->m_multiBody->getBaseVel()[1]);
+				jointVelocitiesQdot.push_back(body->m_multiBody->getBaseVel()[2]);
+				jointVelocitiesQdot.push_back(body->m_multiBody->getBaseOmega()[0]);
+				jointVelocitiesQdot.push_back(body->m_multiBody->getBaseOmega()[1]);
+				jointVelocitiesQdot.push_back(body->m_multiBody->getBaseOmega()[2]);
+				jointVelocitiesQdot.push_back(0);
+#else
+				for (int i = 0; i < 7; i++)
+				{
+					jointPositionsQ.push_back(0);
+					jointVelocitiesQdot.push_back(0);
+				}
+				jointPositionsQ[6] = 1;
+#endif
+				for (int i = 0; i < body->m_multiBody->getNumLinks(); i++)
+				{
+					switch (body->m_multiBody->getLink(i).m_jointType)
+					{
+					case btMultibodyLink::eSpherical:
+					{
+						btScalar* jointPos = body->m_multiBody->getJointPosMultiDof(i);
+						jointPositionsQ.push_back(jointPos[0]);
+						jointPositionsQ.push_back(jointPos[1]);
+						jointPositionsQ.push_back(jointPos[2]);
+						jointPositionsQ.push_back(jointPos[3]);
+						btScalar* jointVel = body->m_multiBody->getJointVelMultiDof(i);
+						jointVelocitiesQdot.push_back(jointVel[0]);
+						jointVelocitiesQdot.push_back(jointVel[1]);
+						jointVelocitiesQdot.push_back(jointVel[2]);
+						jointVelocitiesQdot.push_back(0);
+						break;
+					}
+					case btMultibodyLink::ePrismatic:
+					case btMultibodyLink::eRevolute:
+					{
+						btScalar* jointPos = body->m_multiBody->getJointPosMultiDof(i);
+						jointPositionsQ.push_back(jointPos[0]);
+						btScalar* jointVel = body->m_multiBody->getJointVelMultiDof(i);
+						jointVelocitiesQdot.push_back(jointVel[0]);
+						break;
+					}
+					case btMultibodyLink::eFixed:
+					{
+						//skip
+						break;
+					}
+					default:
+					{
+						b3Error("Unsupported joint type");
+						btAssert(0);
+					}
+					}
+				}
+
+
+				cRBDModel* rbdModel = 0;
+				
+				{
+					BT_PROFILE("findOrCreateRBDModel");
+					rbdModel = m_data->findOrCreateRBDModel(body->m_multiBody, &jointPositionsQ[0], &jointVelocitiesQdot[0]);
+				}
+				if (rbdModel)
+				{
+
+					int num_dof = jointPositionsQ.size();
+					const Eigen::VectorXd& pose = rbdModel->GetPose();
+					const Eigen::VectorXd& vel = rbdModel->GetVel();
+
+					Eigen::Map<Eigen::VectorXd> mKp((double*)clientCmd.m_sendDesiredStateCommandArgument.m_Kp, num_dof);
+					Eigen::Map<Eigen::VectorXd> mKd((double*)clientCmd.m_sendDesiredStateCommandArgument.m_Kd, num_dof);
+					Eigen::Map<Eigen::VectorXd> maxForce((double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque, num_dof);
+
+					Eigen::DiagonalMatrix<double, Eigen::Dynamic> Kp_mat = mKp.asDiagonal();
+					Eigen::DiagonalMatrix<double, Eigen::Dynamic> Kd_mat = mKd.asDiagonal();
+
+					Eigen::MatrixXd M = rbdModel->GetMassMat();
+					//rbdModel->UpdateBiasForce();
+					const Eigen::VectorXd& C = rbdModel->GetBiasForce();
+					M.diagonal() += m_data->m_physicsDeltaTime * mKd;
+
+					Eigen::VectorXd pose_inc;
+					
+					const Eigen::MatrixXd& joint_mat = rbdModel->GetJointMat();
+					{
+						BT_PROFILE("cKinTree::VelToPoseDiff");
+						cKinTree::VelToPoseDiff(joint_mat, rbdModel->GetPose(), rbdModel->GetVel(), pose_inc);
+					}
+
+					//tar_pose needs to be reshuffled?
+					Eigen::VectorXd tar_pose, tar_vel;
+
+					{
+						BT_PROFILE("convertPose");
+							PhysicsServerCommandProcessorInternalData::convertPose(body->m_multiBody,
+							(double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ,
+								(double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot,
+								tar_pose, tar_vel);
+					}
+
+					pose_inc = pose+m_data->m_physicsDeltaTime * pose_inc;
+					
+					{
+						BT_PROFILE("cKinTree::PostProcessPose");
+						cKinTree::PostProcessPose(joint_mat, pose_inc);
+					}
+
+					Eigen::VectorXd pose_err;
+					{
+						BT_PROFILE("cKinTree::CalcVel");
+						cKinTree::CalcVel(joint_mat, pose_inc, tar_pose, 1, pose_err);
+					}
+					for (int i = 0; i < 7; i++)
+					{
+						pose_err[i] = 0;
+					}
+					
+					Eigen::VectorXd vel_err = tar_vel - vel;
+					Eigen::VectorXd acc;
+					
+					{
+						BT_PROFILE("acc");
+						acc = Kp_mat * pose_err + Kd_mat * vel_err - C;
+					}
+
+					{
+						BT_PROFILE("M.ldlt().solve");
+						acc = M.ldlt().solve(acc);
+					}
+					Eigen::VectorXd out_tau = Eigen::VectorXd::Zero(num_dof);
+					out_tau += Kp_mat * pose_err + Kd_mat * (vel_err - m_data->m_physicsDeltaTime * acc);
+					//clamp the forces
+					out_tau = out_tau.cwiseMax(-maxForce);
+					out_tau = out_tau.cwiseMin(maxForce);
+					//apply the forces
+					int torqueIndex = 7;
+					for (int link = 0; link < mb->getNumLinks(); link++)
+					{
+						int dofCount = mb->getLink(link).m_dofCount;
+						int dofOffset = mb->getLink(link).m_dofOffset;
+						if (dofCount == 3)
+						{
+							for (int dof = 0; dof < 3; dof++)
+							{
+								double torque = out_tau[torqueIndex+ dof];
+								mb->addJointTorqueMultiDof(link, dof, torque);
+							}
+							torqueIndex += 4;
+						}
+						if (dofCount == 1)
+						{
+							double torque = out_tau[torqueIndex];
+							mb->addJointTorqueMultiDof(link, 0, torque);
+							torqueIndex++;
+						}
+					}
+					
+					
+				}
+			
+				break;
+			}
+#endif
 			default:
 			{
 				b3Warning("m_controlMode not implemented yet");
@@ -9127,6 +9318,7 @@ bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(cons
 	return hasStatus;
 }
 
+
 bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
 {
 	bool hasStatus = true;
@@ -9370,8 +9562,9 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru
 					&zeroVel[0]);
 				if (rbdModel)
 				{
-					Eigen::MatrixXd out_mass;
+					//Eigen::MatrixXd out_mass;
-					cRBDUtil::BuildMassMat(*rbdModel, out_mass);
+					//cRBDUtil::BuildMassMat(*rbdModel, out_mass);
+					const Eigen::MatrixXd& out_mass = rbdModel->GetMassMat();
 					int skipDofs = 0;// 7 - baseDofQ;
 					int totDofs = dof;
 					serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs-skipDofs;

examples/SharedMemory/SharedMemoryPublic.h

@@ -772,6 +772,7 @@ enum
 	CONTROL_MODE_TORQUE,
 	CONTROL_MODE_POSITION_VELOCITY_PD,
 	CONTROL_MODE_PD,  // The standard PD control implemented as soft constraint.
+	CONTROL_MODE_STABLE_PD,
 };
 
 ///flags for b3ApplyExternalTorque and b3ApplyExternalForce

examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp

@@ -1,7 +1,7 @@
 #include "RBDModel.h"
 #include "RBDUtil.h"
 #include "KinTree.h"
-
+#include "LinearMath/btQuickprof.h"
 cRBDModel::cRBDModel()
 {
 }
@@ -38,12 +38,27 @@ void cRBDModel::Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel)
 	SetPose(pose);
 	SetVel(vel);
 
+	{
+		BT_PROFILE("rbdModel::UpdateJointSubspaceArr");
 		UpdateJointSubspaceArr();
+	}
+	{
+		BT_PROFILE("rbdModel::UpdateChildParentMatArr");
 		UpdateChildParentMatArr();
+	}
+	{
+		BT_PROFILE("rbdModel::UpdateSpWorldTrans");
 		UpdateSpWorldTrans();
+	}
+	{
+		BT_PROFILE("UpdateMassMat");
 		UpdateMassMat();
+	}
+	{
+		BT_PROFILE("UpdateBiasForce");
 		UpdateBiasForce();
 	}
+}
 
 int cRBDModel::GetNumDof() const
 {

examples/Utils/ChromeTraceUtil.cpp

@@ -50,6 +50,11 @@ struct btTimings
 
 			m_firstTiming = false;
 
+			if (startTime > endTime)
+			{
+				endTime = startTime;
+			}
+
 			unsigned long long int startTimeDiv1000 = startTime / 1000;
 			unsigned long long int endTimeDiv1000 = endTime / 1000;
 
@@ -62,10 +67,7 @@ struct btTimings
 
 #else
 
-			if (startTime > endTime)
+			
-			{
-				endTime = startTime;
-			}
 			unsigned int startTimeRem1000 = startTime % 1000;
 			unsigned int endTimeRem1000 = endTime % 1000;
 

examples/pybullet/examples/humanoidMotionCapture.py

@@ -21,10 +21,10 @@ from pdControllerStable import PDControllerStableMultiDof
 explicitPD = PDControllerExplicitMultiDof(p)
 stablePD = PDControllerStableMultiDof(p)
 
-p.resetDebugVisualizerCamera(cameraDistance=7,
+p.resetDebugVisualizerCamera(cameraDistance=7.4,
                              cameraYaw=-94,
                              cameraPitch=-14,
-                             cameraTargetPosition=[0.31, 0.03, -1.16])
+                             cameraTargetPosition=[0.24, -0.02, -0.09])
 
 import pybullet_data
 p.setTimeOut(10000)
@@ -67,7 +67,7 @@ jointTypes = [
     "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED"
 ]
 
-startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4]]
+startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4], [0, 0, 4]]
 
 p.addUserDebugText("Stable PD",
                    [startLocations[0][0], startLocations[0][1] + 1, startLocations[0][2]],
@@ -81,6 +81,9 @@ p.addUserDebugText("Explicit PD",
 p.addUserDebugText("Kinematic",
                    [startLocations[3][0], startLocations[3][1] + 1, startLocations[3][2]],
                    [0, 0, 0])
+p.addUserDebugText("Stable PD (Py)",
+                   [startLocations[4][0], startLocations[4][1] + 1, startLocations[4][2]],
+                   [0, 0, 0])
 
 humanoid = p.loadURDF("humanoid/humanoid.urdf",
                       startLocations[0],
@@ -102,6 +105,11 @@ humanoid4 = p.loadURDF("humanoid/humanoid.urdf",
                        globalScaling=0.25,
                        useFixedBase=False,
                        flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid5 = p.loadURDF("humanoid/humanoid.urdf",
+                      startLocations[4],
+                      globalScaling=0.25,
+                      useFixedBase=False,
+                      flags=p.URDF_MAINTAIN_LINK_ORDER)
 
 humanoid_fix = p.createConstraint(humanoid, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
                                   startLocations[0], [0, 0, 0, 1])
@@ -111,6 +119,8 @@ humanoid3_fix = p.createConstraint(humanoid3, -1, -1, -1, p.JOINT_FIXED, [0, 0,
                                    startLocations[2], [0, 0, 0, 1])
 humanoid3_fix = p.createConstraint(humanoid4, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
                                    startLocations[3], [0, 0, 0, 1])
+humanoid4_fix = p.createConstraint(humanoid5, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0],
+                                   startLocations[4], [0, 0, 0, 1])
 
 startPose = [
     2, 0.847532, 0, 0.9986781045, 0.01410400148, -0.0006980000731, -0.04942300517, 0.9988133229,
@@ -135,6 +145,7 @@ p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1])
 p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1])
 p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1])
 p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1])
+p.resetBasePositionAndOrientation(humanoid5, startLocations[4], [0, 0, 0, 1])
 
 index0 = 7
 for j in range(p.getNumJoints(humanoid)):
@@ -150,6 +161,7 @@ for j in range(p.getNumJoints(humanoid)):
     print("spherical position: ", targetPosition)
     print("spherical velocity: ", targetVel)
     p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
+    p.resetJointStateMultiDof(humanoid5, j, targetValue=targetPosition, targetVelocity=targetVel)
     p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel)
   if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE):
     targetPosition = [startPose[index0]]
@@ -158,6 +170,7 @@ for j in range(p.getNumJoints(humanoid)):
     print("revolute:", targetPosition)
     print("revolute	velocity:", targetVel)
     p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel)
+    p.resetJointStateMultiDof(humanoid5, j, targetValue=targetPosition, targetVelocity=targetVel)
     p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel)
 
 for j in range(p.getNumJoints(humanoid)):
@@ -174,6 +187,14 @@ for j in range(p.getNumJoints(humanoid)):
                                    positionGain=0,
                                    velocityGain=1,
                                    force=[0, 0, 0])
+    p.setJointMotorControlMultiDof(humanoid5,
+                                   j,
+                                   p.POSITION_CONTROL,
+                                   targetPosition,
+                                   targetVelocity=[0, 0, 0],
+                                   positionGain=0,
+                                   velocityGain=1,
+                                   force=[0, 0, 0])
     p.setJointMotorControlMultiDof(humanoid3,
                                    j,
                                    p.POSITION_CONTROL,
@@ -195,6 +216,7 @@ for j in range(p.getNumJoints(humanoid)):
     p.setJointMotorControl2(humanoid, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
     p.setJointMotorControl2(humanoid3, j, p.VELOCITY_CONTROL, targetVelocity=0, force=31)
     p.setJointMotorControl2(humanoid4, j, p.VELOCITY_CONTROL, targetVelocity=0, force=10)
+    p.setJointMotorControl2(humanoid5, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0)
 
   #print(ji)
   print("joint[", j, "].type=", jointTypes[ji[2]])
@@ -412,7 +434,8 @@ while (p.isConnected()):
     #for po	in pose:
     #	print(po)
     
-    taus = stablePD.computePD(bodyUniqueId=humanoid,
+    
+    taus = stablePD.computePD(bodyUniqueId=humanoid5,
                                 jointIndices=jointIndicesAll,
                                 desiredPositions=pose,
                                 desiredVelocities=[0] * totalDofs,
@@ -421,6 +444,32 @@ while (p.isConnected()):
                                 maxForces=[maxForce] * totalDofs,
                                 timeStep=timeStep)
     
+    indices = [chest, neck, rightHip, rightKnee,
+              rightAnkle, rightShoulder, rightElbow,
+               leftHip, leftKnee, leftAnkle,
+               leftShoulder, leftElbow]
+    targetPositions = [chestRot,neckRot,rightHipRot, rightKneeRot,
+                      rightAnkleRot, rightShoulderRot, rightElbowRot,
+                      leftHipRot, leftKneeRot, leftAnkleRot,
+                      leftShoulderRot, leftElbowRot]
+    maxForces = [  [maxForce,maxForce,maxForce], [maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce],
+                  [maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce],
+                  [maxForce,maxForce,maxForce], [maxForce], [maxForce,maxForce,maxForce],
+                  [maxForce,maxForce,maxForce], [maxForce]]
+    
+   
+    kps = [1000]*12
+    kds = [100]*12
+    
+                      
+    p.setJointMotorControlMultiDofArray(humanoid,
+                                   indices,
+                                   p.STABLE_PD_CONTROL,
+                                   targetPositions=targetPositions,
+                                   positionGains=kps,
+                                   velocityGains=kds,
+                                   forces=maxForces)
+
     taus3 = explicitPD.computePD(bodyUniqueId=humanoid3,
                                  jointIndices=jointIndicesAll,
                                  desiredPositions=pose,
@@ -437,7 +486,7 @@ while (p.isConnected()):
       if jointDofCounts[index] == 4:
         
         p.setJointMotorControlMultiDof(
-            humanoid,
+              humanoid5,
             jointIndex,
             p.TORQUE_CONTROL,
             force=[taus[dofIndex + 0], taus[dofIndex + 1], taus[dofIndex + 2]])
@@ -449,7 +498,8 @@ while (p.isConnected()):
 
       if jointDofCounts[index] == 1:
 
-        p.setJointMotorControlMultiDof(humanoid,
+        
+        p.setJointMotorControlMultiDof(humanoid5,
                                        jointIndex,
                                        controlMode=p.TORQUE_CONTROL,
                                        force=[taus[dofIndex]])

examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py

@@ -153,11 +153,26 @@ class HumanoidStablePD(object):
 
   def initializePose(self, pose, phys_model, initBase, initializeVelocity=True):
     
+    useArray = True
     if initializeVelocity:
       if initBase:
         self._pybullet_client.resetBasePositionAndOrientation(phys_model, pose._basePos,
                                                               pose._baseOrn)
         self._pybullet_client.resetBaseVelocity(phys_model, pose._baseLinVel, pose._baseAngVel)
+      if useArray:
+        indices = [chest,neck,rightHip,rightKnee,
+                  rightAnkle, rightShoulder, rightElbow,leftHip,
+                  leftKnee, leftAnkle, leftShoulder,leftElbow]
+        jointPositions = [pose._chestRot, pose._neckRot, pose._rightHipRot, pose._rightKneeRot,
+                          pose._rightAnkleRot, pose._rightShoulderRot, pose._rightElbowRot, pose._leftHipRot,
+                          pose._leftKneeRot, pose._leftAnkleRot, pose._leftShoulderRot, pose._leftElbowRot]
+        
+        jointVelocities = [pose._chestVel, pose._neckVel, pose._rightHipVel, pose._rightKneeVel,
+                          pose._rightAnkleVel, pose._rightShoulderVel, pose._rightElbowVel, pose._leftHipVel,
+                          pose._leftKneeVel, pose._leftAnkleVel, pose._leftShoulderVel, pose._leftElbowVel]
+        self._pybullet_client.resetJointStatesMultiDof(phys_model, indices,
+                                                    jointPositions, jointVelocities)
+      else:
         self._pybullet_client.resetJointStateMultiDof(phys_model, chest, pose._chestRot,
                                                       pose._chestVel)
         self._pybullet_client.resetJointStateMultiDof(phys_model, neck, pose._neckRot, pose._neckVel)
@@ -182,9 +197,20 @@ class HumanoidStablePD(object):
         self._pybullet_client.resetJointStateMultiDof(phys_model, leftElbow, pose._leftElbowRot,
                                                       pose._leftElbowVel)
     else:
+      
       if initBase:
         self._pybullet_client.resetBasePositionAndOrientation(phys_model, pose._basePos,
                                                               pose._baseOrn)
+      if useArray:
+        indices = [chest,neck,rightHip,rightKnee,
+                  rightAnkle, rightShoulder, rightElbow,leftHip,
+                  leftKnee, leftAnkle, leftShoulder,leftElbow]
+        jointPositions = [pose._chestRot, pose._neckRot, pose._rightHipRot, pose._rightKneeRot,
+                          pose._rightAnkleRot, pose._rightShoulderRot, pose._rightElbowRot, pose._leftHipRot,
+                          pose._leftKneeRot, pose._leftAnkleRot, pose._leftShoulderRot, pose._leftElbowRot]
+        self._pybullet_client.resetJointStatesMultiDof(phys_model, indices,jointPositions)
+        
+      else:
         self._pybullet_client.resetJointStateMultiDof(phys_model, chest, pose._chestRot, [0, 0, 0])
         self._pybullet_client.resetJointStateMultiDof(phys_model, neck, pose._neckRot, [0, 0, 0])
         self._pybullet_client.resetJointStateMultiDof(phys_model, rightHip, pose._rightHipRot,
@@ -275,6 +301,57 @@ class HumanoidStablePD(object):
     pose = self._poseInterpolator.ConvertFromAction(self._pybullet_client, action)
     return pose
 
+  def computeAndApplyPDForces(self, desiredPositions, maxForces):
+    dofIndex = 7
+    scaling = 1
+    indices = []
+    forces = []
+    targetPositions=[]
+    targetVelocities=[]
+    kps = []
+    kds = []
+    
+    for index in range(len(self._jointIndicesAll)):
+      jointIndex = self._jointIndicesAll[index]
+      indices.append(jointIndex)
+      kps.append(self._kpOrg[dofIndex])
+      kds.append(self._kdOrg[dofIndex])
+      if self._jointDofCounts[index] == 4:
+        force = [
+            scaling * maxForces[dofIndex + 0],
+            scaling * maxForces[dofIndex + 1],
+            scaling * maxForces[dofIndex + 2]
+        ]
+        targetVelocity = [0,0,0]
+        targetPosition = [
+            desiredPositions[dofIndex + 0],
+            desiredPositions[dofIndex + 1],
+            desiredPositions[dofIndex + 2],
+            desiredPositions[dofIndex + 3]
+        ]
+      if self._jointDofCounts[index] == 1:
+        force = [scaling * maxForces[dofIndex]]
+        targetPosition = [desiredPositions[dofIndex+0]]
+        targetVelocity = [0]
+      forces.append(force)
+      targetPositions.append(targetPosition)
+      targetVelocities.append(targetVelocity)
+      dofIndex += self._jointDofCounts[index]
+      
+    #static char* kwlist[] = { "bodyUniqueId", 
+    #"jointIndices", 
+    #"controlMode", "targetPositions", "targetVelocities", "forces", "positionGains", "velocityGains", "maxVelocities", "physicsClientId", NULL };
+	
+    self._pybullet_client.setJointMotorControlMultiDofArray(self._sim_model,
+                                                           indices,
+                                                           self._pybullet_client.STABLE_PD_CONTROL,
+                                                           targetPositions = targetPositions,
+                                                           targetVelocities = targetVelocities,
+                                                           forces=forces,
+                                                           positionGains = kps,
+                                                           velocityGains = kds,
+                                                           )
+
   def computePDForces(self, desiredPositions, desiredVelocities, maxForces):
     if desiredVelocities == None:
       desiredVelocities = [0] * self._totalDofs
@@ -292,6 +369,29 @@ class HumanoidStablePD(object):
   def applyPDForces(self, taus):
     dofIndex = 7
     scaling = 1
+    useArray = True
+    indices = []
+    forces = []
+    
+    if (useArray):
+      for index in range(len(self._jointIndicesAll)):
+        jointIndex = self._jointIndicesAll[index]
+        indices.append(jointIndex)
+        if self._jointDofCounts[index] == 4:
+          force = [
+              scaling * taus[dofIndex + 0], scaling * taus[dofIndex + 1],
+              scaling * taus[dofIndex + 2]
+          ]
+        if self._jointDofCounts[index] == 1:
+          force = [scaling * taus[dofIndex]]
+          #print("force[", jointIndex,"]=",force)
+        forces.append(force)
+        dofIndex += self._jointDofCounts[index]
+      self._pybullet_client.setJointMotorControlMultiDofArray(self._sim_model,
+                                                             indices,
+                                                             self._pybullet_client.TORQUE_CONTROL,
+                                                             forces=forces)
+    else:
       for index in range(len(self._jointIndicesAll)):
         jointIndex = self._jointIndicesAll[index]
         if self._jointDofCounts[index] == 4:
@@ -531,10 +631,17 @@ class HumanoidStablePD(object):
     #self.pb2dmJoints=[0,1,2,9,10,11,3,4,5,12,13,14,6,7,8]
     self.pb2dmJoints = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14]
 
+    linkIndicesSim = []
+    for pbJoint in range(self._pybullet_client.getNumJoints(self._sim_model)):
+      linkIndicesSim.append(self.pb2dmJoints[pbJoint])
+      
+    linkStatesSim = self._pybullet_client.getLinkStates(self._sim_model, linkIndicesSim, computeForwardKinematics=True, computeLinkVelocity=True)
+    
     for pbJoint in range(self._pybullet_client.getNumJoints(self._sim_model)):
       j = self.pb2dmJoints[pbJoint]
       #print("joint order:",j)
-      ls = self._pybullet_client.getLinkState(self._sim_model, j, computeForwardKinematics=True)
+      #ls = self._pybullet_client.getLinkState(self._sim_model, j, computeForwardKinematics=True)
+      ls = linkStatesSim[pbJoint]
       linkPos = ls[0]
       linkOrn = ls[1]
       linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms(
@@ -560,9 +667,12 @@ class HumanoidStablePD(object):
       stateVector.append(linkOrnLocal[1])
       stateVector.append(linkOrnLocal[2])
 
+       
     for pbJoint in range(self._pybullet_client.getNumJoints(self._sim_model)):
       j = self.pb2dmJoints[pbJoint]
-      ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True)
+      #ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True)
+      ls = linkStatesSim[pbJoint]
+      
       linkLinVel = ls[6]
       linkAngVel = ls[7]
       linkLinVelLocal, unused = self._pybullet_client.multiplyTransforms([0, 0, 0], rootTransOrn,
@@ -706,15 +816,26 @@ class HumanoidStablePD(object):
     root_ang_vel_err = self.calcRootAngVelErr(angVelSim, angVelKin)
     vel_err += root_rot_w * root_ang_vel_err
 
+    useArray = False
+    
+    if useArray:
+      jointIndices = range(num_joints)
+      simJointStates = self._pybullet_client.getJointStatesMultiDof(self._sim_model, jointIndices)
+      kinJointStates = self._pybullet_client.getJointStatesMultiDof(self._kin_model, jointIndices)
     for j in range(num_joints):
       curr_pose_err = 0
       curr_vel_err = 0
       w = mJointWeights[j]
-
+      if useArray:
+        simJointInfo = simJointStates[j]
+      else:
         simJointInfo = self._pybullet_client.getJointStateMultiDof(self._sim_model, j)
 
       #print("simJointInfo.pos=",simJointInfo[0])
       #print("simJointInfo.vel=",simJointInfo[1])
+      if useArray:
+        kinJointInfo = kinJointStates[j]
+      else:
         kinJointInfo = self._pybullet_client.getJointStateMultiDof(self._kin_model, j)
       #print("kinJointInfo.pos=",kinJointInfo[0])
       #print("kinJointInfo.vel=",kinJointInfo[1])

examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py

@@ -52,7 +52,7 @@ class PyBulletDeepMimicEnv(Env):
       motionPath = pybullet_data.getDataPath() + "/" + motion_file[0]
       #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
       self._mocapData.Load(motionPath)
-      timeStep = 1. / 600.
+      timeStep = 1. / 240.
       useFixedBase = False
       self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData,
                                                            timeStep, useFixedBase)
@@ -283,10 +283,16 @@ class PyBulletDeepMimicEnv(Env):
         ]
 
         if self._useStablePD:
+          usePythonStablePD = False
+          if usePythonStablePD:
             taus = self._humanoid.computePDForces(self.desiredPose,
                                                 desiredVelocities=None,
                                                 maxForces=maxForces)
+            #taus = [0]*43
             self._humanoid.applyPDForces(taus)
+          else:
+            self._humanoid.computeAndApplyPDForces(self.desiredPose,
+                                                maxForces=maxForces)
         else:
           self._humanoid.setJointMotors(self.desiredPose, maxForces=maxForces)
 

examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py

@@ -47,7 +47,7 @@ def isKeyTriggered(keys, key):
 
 animating = False
 singleStep = False
-
+humanoid.resetPose()
 t = 0
 while (1):
 
@@ -66,7 +66,7 @@ while (1):
     #print("t=",t)
     for i in range(1):
 
-      print("t=", t)
+      #print("t=", t)
       humanoid.setSimTime(t)
 
       humanoid.computePose(humanoid._frameFraction)
@@ -75,9 +75,11 @@ while (1):
       #humanoid.resetPose()
 
       desiredPose = humanoid.computePose(humanoid._frameFraction)
+      
       #desiredPose = desiredPose.GetPose()
       #curPose = HumanoidPoseInterpolator()
       #curPose.reset()
+      
       s = humanoid.getState()
       #np.savetxt("pb_record_state_s.csv", s, delimiter=",")
       maxForces = [
@@ -85,10 +87,14 @@ while (1):
           90, 90, 100, 100, 100, 100, 60, 200, 200, 200, 200, 150, 90, 90, 90, 90, 100, 100, 100,
           100, 60
       ]
-      taus = humanoid.computePDForces(desiredPose, desiredVelocities=None, maxForces=maxForces)
       
-      #print("taus=",taus)
+      usePythonStablePD = False
+      if usePythonStablePD:
+        taus = humanoid.computePDForces(desiredPose, desiredVelocities=None, maxForces=maxForces)
+        #Print("taus=",taus)
         humanoid.applyPDForces(taus)
+      else:
+        humanoid.computeAndApplyPDForces(desiredPose,maxForces=maxForces)
 
       pybullet_client.stepSimulation()
       t += 1. / 600.

examples/pybullet/gym/pybullet_utils/pd_controller_stable.py

@@ -62,7 +62,14 @@ class PDControllerStableMultiDof(object):
     qIndex = 7
     qdotIndex = 7
     zeroAccelerations = [0, 0, 0, 0, 0, 0, 0]
+    useArray = True
+    if useArray:
+      jointStates = self._pb.getJointStatesMultiDof(bodyUniqueId,jointIndices)
+    
     for i in range(numJoints):
+      if useArray:
+        js = jointStates[i]
+      else:
         js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
 
       jointPos = js[0]
@@ -153,8 +160,6 @@ class PDControllerStableMultiDof(object):
     if useNumpySolver:
       qddot = np.linalg.solve(A, b)
     else:
-      dofCount = len(b)
-      print(dofCount)
       qddot = self._pb.ldltSolve(bodyUniqueId, jointPositions=q1, b=b.tolist(), kd=kds, t=timeStep)
 
     tau = p + d - Kd.dot(qddot) * timeStep