Merge pull request #2203 from erwincoumans/master

implement stablePD control version of testLaikago,  fix getCameraImage in VR,  only report solver analytics if enabled using setPhysicsEngineParameter

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -689,6 +689,17 @@ B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCom
 	return 0;
 }
 
+B3_SHARED_API int b3PhysicsParamSetSolverAnalytics(b3SharedMemoryCommandHandle commandHandle, int reportSolverAnalytics)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
+	b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+	command->m_physSimParamArgs.m_reportSolverAnalytics = reportSolverAnalytics;
+	command->m_updateFlags |= SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS;
+	return 0;
+}
+
+
+
 B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
 {
 	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -2214,6 +2225,19 @@ B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
 	return CMD_INVALID_STATUS;
 }
 
+B3_SHARED_API int b3GetStatusForwardDynamicsAnalyticsData(b3SharedMemoryStatusHandle statusHandle, struct b3ForwardDynamicsAnalyticsArgs* analyticsData)
+{
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	//b3Assert(status);
+	if (status)
+	{
+		*analyticsData = status->m_forwardDynamicsAnalyticsArgs;
+		return status->m_forwardDynamicsAnalyticsArgs.m_numIslands;
+	}
+	return 0;
+}
+
+
 B3_SHARED_API int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
 {
 	int numBodies = 0;

examples/SharedMemory/PhysicsClientC_API.h

@@ -349,7 +349,8 @@ extern "C"
 	B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT);
 	B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType(b3SharedMemoryCommandHandle commandHandle, int constraintSolverType);
 	B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize(b3SharedMemoryCommandHandle commandHandle, int minimumSolverIslandSize);
-
+	B3_SHARED_API int b3PhysicsParamSetSolverAnalytics(b3SharedMemoryCommandHandle commandHandle, int reportSolverAnalytics);
+	
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient);
 	B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle, struct b3PhysicsSimulationParameters* params);
 
@@ -360,6 +361,9 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
 
+	B3_SHARED_API int b3GetStatusForwardDynamicsAnalyticsData(b3SharedMemoryStatusHandle statusHandle, struct b3ForwardDynamicsAnalyticsArgs* analyticsData);
+
+
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -7605,17 +7605,17 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
 	}
 
 	btScalar deltaTimeScaled = m_data->m_physicsDeltaTime * simTimeScalingFactor;
-
+	
 	int numSteps = 0;
 	if (m_data->m_numSimulationSubSteps > 0)
 	{
 		numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
-                m_data->m_simulationTimestamp += deltaTimeScaled;
+		m_data->m_simulationTimestamp += deltaTimeScaled;
 	}
 	else
 	{
 		numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
-                m_data->m_simulationTimestamp += deltaTimeScaled;
+		m_data->m_simulationTimestamp += deltaTimeScaled;
 	}
 
 	if (numSteps > 0)
@@ -7624,6 +7624,24 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
 	}
 
 	SharedMemoryStatus& serverCmd = serverStatusOut;
+
+	serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSteps = numSteps;
+
+	btAlignedObjectArray<btSolverAnalyticsData> islandAnalyticsData;
+
+	m_data->m_dynamicsWorld->getAnalyticsData(islandAnalyticsData);
+	serverCmd.m_forwardDynamicsAnalyticsArgs.m_numIslands = islandAnalyticsData.size();
+	int numIslands = btMin(islandAnalyticsData.size(), MAX_ISLANDS_ANALYTICS);
+	
+	for (int i=0;i<numIslands;i++)
+	{
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSolverCalls = islandAnalyticsData[i].m_numSolverCalls;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_islandId = islandAnalyticsData[i].m_islandId;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numBodies = islandAnalyticsData[i].m_numBodies;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numIterationsUsed = islandAnalyticsData[i].m_numIterationsUsed;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_remainingLeastSquaresResidual = islandAnalyticsData[i].m_remainingLeastSquaresResidual;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numContactManifolds = islandAnalyticsData[i].m_numContactManifolds;
+	}
 	serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
 
 	return hasStatus;
@@ -8408,6 +8426,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 		m_data->m_pluginManager.getFileIOInterface()->enableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching!=0);
 	}
 
+	if (clientCmd.m_updateFlags & SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS)
+	{
+		m_data->m_dynamicsWorld->getSolverInfo().m_reportSolverAnalytics = clientCmd.m_physSimParamArgs.m_reportSolverAnalytics;
+	}
+	
 	SharedMemoryStatus& serverCmd = serverStatusOut;
 	serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
 	return hasStatus;

examples/SharedMemory/SharedMemoryCommands.h

@@ -456,31 +456,32 @@ enum EnumSimDesiredStateUpdateFlags
 enum EnumSimParamUpdateFlags
 {
 	SIM_PARAM_UPDATE_DELTA_TIME = 1,
-	SIM_PARAM_UPDATE_GRAVITY = 2,
+	SIM_PARAM_UPDATE_GRAVITY = 1<<1,
-	SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS = 4,
+	SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS = 1<<2,
-	SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS = 8,
+	SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS = 1<<3,
-	SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16,
+	SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 1<<4,
-	SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP = 32,
+	SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP = 1<<5,
-	SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS = 64,
+	SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS = 1<<6,
-	SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE = 128,
+	SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE = 1<<7,
-	SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256,
+	SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 1<<8,
-	SIM_PARAM_UPDATE_COLLISION_FILTER_MODE = 512,
+	SIM_PARAM_UPDATE_COLLISION_FILTER_MODE = 1 << 9,
-	SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024,
+	SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1 << 10,
-	SIM_PARAM_ENABLE_CONE_FRICTION = 2048,
+	SIM_PARAM_ENABLE_CONE_FRICTION = 1 << 11,
-	SIM_PARAM_ENABLE_FILE_CACHING = 4096,
+	SIM_PARAM_ENABLE_FILE_CACHING = 1 << 12,
-	SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192,
+	SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 1 << 13,
-	SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP = 16384,
+	SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP = 1 << 14,
-	SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768,
+	SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 1 << 15,
-	SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536,
+	SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 1 << 16,
-	SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072,
+	SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 1 << 17,
-	SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 262144,
+	SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 1 << 18,
-	SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM = 524288,
+	SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM = 1 << 19,
-	SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1048576,
+	SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1 << 20,
-	SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 2097152,
+	SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 1 << 21,
-	SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304,
+	SIM_PARAM_UPDATE_CONTACT_SLOP = 1 << 22,
-	SIM_PARAM_ENABLE_SAT = 8388608,
+	SIM_PARAM_ENABLE_SAT = 1 << 23,
-	SIM_PARAM_CONSTRAINT_SOLVER_TYPE = 16777216,
+	SIM_PARAM_CONSTRAINT_SOLVER_TYPE = 1 << 24,
-	SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 33554432,
+	SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 1 << 25,
+	SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26,
 
 };
 
@@ -957,6 +958,8 @@ struct b3CreateUserShapeArgs
 	b3CreateUserShapeData m_shapes[MAX_COMPOUND_COLLISION_SHAPES];
 };
 
+
+
 struct b3CreateUserShapeResultArgs
 {
 	int m_userShapeUniqueId;
@@ -1180,6 +1183,7 @@ struct SharedMemoryStatus
 		struct SyncUserDataArgs m_syncUserDataArgs;
 		struct UserDataResponseArgs m_userDataResponseArgs;
 		struct UserDataRequestArgs m_removeUserDataResponseArgs;
+		struct b3ForwardDynamicsAnalyticsArgs m_forwardDynamicsAnalyticsArgs;
 	};
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -903,7 +903,7 @@ struct b3PluginArguments
 struct b3PhysicsSimulationParameters
 {
 	double m_deltaTime;
-        double m_simulationTimestamp;  // Output only timestamp of simulation.
+	double m_simulationTimestamp;  // user logging timestamp of simulation.
 	double m_gravityAcceleration[3];
 	int m_numSimulationSubSteps;
 	int m_numSolverIterations;
@@ -929,8 +929,10 @@ struct b3PhysicsSimulationParameters
 	int m_enableSAT;
 	int m_constraintSolverType;
 	int m_minimumSolverIslandSize;
+	int m_reportSolverAnalytics;
 };
 
+
 enum eConstraintSolverTypes
 {
 	eConstraintSolverLCP_SI = 1,
@@ -941,6 +943,25 @@ enum eConstraintSolverTypes
 	eConstraintSolverLCP_BLOCK_PGS,
 };
 
+struct b3ForwardDynamicsAnalyticsIslandData
+{
+	int m_islandId;
+	int m_numBodies;
+	int m_numContactManifolds;
+	int m_numIterationsUsed;
+	double m_remainingLeastSquaresResidual;
+};
+
+#define MAX_ISLANDS_ANALYTICS 1024
+
+struct b3ForwardDynamicsAnalyticsArgs
+{
+	int m_numSteps;
+	int m_numIslands;
+	int m_numSolverCalls;
+	struct b3ForwardDynamicsAnalyticsIslandData m_islandData[MAX_ISLANDS_ANALYTICS];
+};
+
 enum eFileIOActions
 {
 	eAddFileIOAction = 1024,//avoid collision with eFileIOTypes

examples/StandaloneMain/hellovr_opengl_main.cpp

@@ -1792,6 +1792,7 @@ void CMainApplication::RenderStereoTargets()
 
 	//m_app->drawGrid(gridUp);
 
+	m_app->m_instancingRenderer->setRenderFrameBuffer(0);
 	glBindFramebuffer(GL_FRAMEBUFFER, 0);
 
 	glDisable(GL_MULTISAMPLE);

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

@@ -36,8 +36,11 @@ class QuadrupedPoseInterpolator(object):
     self._baseOrn = bullet_client.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
     self._baseAngVel = self.ComputeAngVel(baseOrn1Start,baseOrn1Next, keyFrameDuration, bullet_client)
     
-    jointPositions=[]
+    jointPositions=[self._basePos[0],self._basePos[1],self._basePos[2],
-    jointVelocities=[]
+       self._baseOrn[0],self._baseOrn[1],self._baseOrn[2],self._baseOrn[3]]
+    jointVelocities=[self._baseLinVel[0],self._baseLinVel[1],self._baseLinVel[2],
+      self._baseAngVel[0],self._baseAngVel[1],self._baseAngVel[2]]
+    
     for j in range (12):
       index=j+8
       jointPosStart=frameData[index]

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

@@ -0,0 +1,577 @@
+
+from pybullet_utils import pd_controller_stable 
+from pybullet_envs.deep_mimic.env import quadruped_pose_interpolator
+import math
+
+
+class QuadrupedStablePD(object):
+  def __init__(self, pybullet_client, mocap_data, timeStep, useFixedBase=True):
+    self._pybullet_client = pybullet_client
+    self._mocap_data = mocap_data
+    print("LOADING quadruped!")
+ 
+    startPos=[0.007058990464444105, 0.03149299192130908, 0.4918981912395484]
+    startOrn=[0.005934649695708604, 0.7065453990917289, 0.7076373820553712, -0.0027774940359030264]
+    self._sim_model = self._pybullet_client.loadURDF("laikago/laikago.urdf",startPos,startOrn, flags = urdfFlags,useFixedBase=False)
+    self._pybullet_client.resetBasePositionAndOrientation(_sim_model,startPos,startOrn)
+   
+    self._end_effectors = []  #ankle and wrist, both left and right
+
+    self._kin_model = self._pybullet_client.loadURDF("laikago/laikago.urdf",startPos,startOrn,useFixedBase=True)
+    
+    self._pybullet_client.changeDynamics(self._sim_model, -1, lateralFriction=0.9)
+    for j in range (self._pybullet_client.getNumJoints(self._sim_model)):
+      self._pybullet_client.changeDynamics(self._sim_model, j, lateralFriction=0.9)
+      
+    self._pybullet_client.changeDynamics(self._sim_model, -1, linearDamping=0, angularDamping=0)
+    self._pybullet_client.changeDynamics(self._kin_model, -1, linearDamping=0, angularDamping=0)
+    
+    #todo: add feature to disable simulation for a particular object. Until then, disable all collisions
+    self._pybullet_client.setCollisionFilterGroupMask(self._kin_model,-1,collisionFilterGroup=0,collisionFilterMask=0)
+    self._pybullet_client.changeDynamics(self._kin_model,-1,activationState=self._pybullet_client.ACTIVATION_STATE_SLEEP+self._pybullet_client.ACTIVATION_STATE_ENABLE_SLEEPING+self._pybullet_client.ACTIVATION_STATE_DISABLE_WAKEUP)
+    alpha = 0.4
+    self._pybullet_client.changeVisualShape(self._kin_model,-1, rgbaColor=[1,1,1,alpha])
+    for j in range (self._pybullet_client.getNumJoints(self._kin_model)):
+      self._pybullet_client.setCollisionFilterGroupMask(self._kin_model,j,collisionFilterGroup=0,collisionFilterMask=0)
+      self._pybullet_client.changeDynamics(self._kin_model,j,activationState=self._pybullet_client.ACTIVATION_STATE_SLEEP+self._pybullet_client.ACTIVATION_STATE_ENABLE_SLEEPING+self._pybullet_client.ACTIVATION_STATE_DISABLE_WAKEUP)
+      self._pybullet_client.changeVisualShape(self._kin_model,j, rgbaColor=[1,1,1,alpha])
+        
+    self._poseInterpolator = humanoid_pose_interpolator.HumanoidPoseInterpolator()
+    
+    for i in range (self._mocap_data.NumFrames()-1):
+      frameData = self._mocap_data._motion_data['Frames'][i]
+      self._poseInterpolator.PostProcessMotionData(frameData)
+      
+    self._stablePD = pd_controller_stable.PDControllerStableMultiDof(self._pybullet_client)
+    self._timeStep = timeStep
+    #todo: kp/pd
+    self._kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300]
+    self._kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30]
+   
+    self._jointIndicesAll = [chest,neck, rightHip,rightKnee,rightAnkle,rightShoulder,rightElbow,leftHip,leftKnee,leftAnkle,leftShoulder,leftElbow]
+    for j in self._jointIndicesAll:
+      #self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, j, self._pybullet_client.POSITION_CONTROL, force=[1,1,1])
+      self._pybullet_client.setJointMotorControl2(self._sim_model, j, self._pybullet_client.POSITION_CONTROL, targetPosition=0, positionGain=0, targetVelocity=0,force=jointFrictionForce)
+      self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, j, self._pybullet_client.POSITION_CONTROL,targetPosition=[0,0,0,1], targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[jointFrictionForce,jointFrictionForce,jointFrictionForce])
+      self._pybullet_client.setJointMotorControl2(self._kin_model, j, self._pybullet_client.POSITION_CONTROL, targetPosition=0, positionGain=0, targetVelocity=0,force=0)
+      self._pybullet_client.setJointMotorControlMultiDof(self._kin_model, j, self._pybullet_client.POSITION_CONTROL,targetPosition=[0,0,0,1], targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[jointFrictionForce,jointFrictionForce,0])
+    
+    self._jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
+    
+    #only those body parts/links are allowed to touch the ground, otherwise the episode terminates
+    self._allowed_body_parts=[5,11]
+    
+    #[x,y,z] base position and [x,y,z,w] base orientation!
+    self._totalDofs = 7 
+    for dof in self._jointDofCounts:
+      self._totalDofs += dof
+    self.setSimTime(0)
+    
+    self.resetPose()
+    
+  def resetPose(self):
+    #print("resetPose with self._frame=", self._frame, " and self._frameFraction=",self._frameFraction)
+    pose = self.computePose(self._frameFraction)
+    self.initializePose(self._poseInterpolator, self._sim_model, initBase=True)
+    self.initializePose(self._poseInterpolator, self._kin_model, initBase=False)
+    
+  def initializePose(self,  pose, phys_model,initBase, initializeVelocity = 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)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,chest,pose._chestRot, pose._chestVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,neck,pose._neckRot, pose._neckVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,rightHip,pose._rightHipRot, pose._rightHipVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,rightKnee,pose._rightKneeRot, pose._rightKneeVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,rightAnkle,pose._rightAnkleRot, pose._rightAnkleVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,rightShoulder,pose._rightShoulderRot, pose._rightShoulderVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,rightElbow, pose._rightElbowRot, pose._rightElbowVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftHip, pose._leftHipRot, pose._leftHipVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftKnee, pose._leftKneeRot, pose._leftKneeVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftShoulder, pose._leftShoulderRot, pose._leftShoulderVel)
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftElbow, pose._leftElbowRot, pose._leftElbowVel)
+    else:
+      if initBase:
+        self._pybullet_client.resetBasePositionAndOrientation(phys_model, pose._basePos, pose._baseOrn)
+      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, [0,0,0])
+      self._pybullet_client.resetJointStateMultiDof(phys_model,rightKnee,pose._rightKneeRot, [0])
+      self._pybullet_client.resetJointStateMultiDof(phys_model,rightAnkle,pose._rightAnkleRot, [0,0,0])
+      self._pybullet_client.resetJointStateMultiDof(phys_model,rightShoulder,pose._rightShoulderRot, [0,0,0])
+      self._pybullet_client.resetJointStateMultiDof(phys_model,rightElbow, pose._rightElbowRot, [0])
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftHip, pose._leftHipRot, [0,0,0])
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftKnee, pose._leftKneeRot, [0])
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftAnkle, pose._leftAnkleRot, [0,0,0])
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftShoulder, pose._leftShoulderRot, [0,0,0])
+      self._pybullet_client.resetJointStateMultiDof(phys_model,leftElbow, pose._leftElbowRot, [0])    
+
+  def calcCycleCount(self, simTime, cycleTime):
+    phases = simTime / cycleTime;
+    count = math.floor(phases)
+    loop = True
+    #count = (loop) ? count : cMathUtil::Clamp(count, 0, 1);
+    return count
+    
+
+  def getCycleTime(self):
+    keyFrameDuration = self._mocap_data.KeyFrameDuraction()
+    cycleTime = keyFrameDuration*(self._mocap_data.NumFrames()-1)
+    return cycleTime
+    
+  def setSimTime(self, t):
+    self._simTime = t
+    #print("SetTimeTime time =",t)
+    keyFrameDuration = self._mocap_data.KeyFrameDuraction()
+    cycleTime = self.getCycleTime()
+    #print("self._motion_data.NumFrames()=",self._mocap_data.NumFrames())
+    self._cycleCount = self.calcCycleCount(t, cycleTime)
+    #print("cycles=",cycles)
+    frameTime = t - self._cycleCount*cycleTime
+    if (frameTime<0):
+      frameTime += cycleTime
+    
+    #print("keyFrameDuration=",keyFrameDuration)  
+    #print("frameTime=",frameTime)
+    self._frame = int(frameTime/keyFrameDuration)
+    #print("self._frame=",self._frame)
+    
+    self._frameNext = self._frame+1
+    if (self._frameNext >=  self._mocap_data.NumFrames()):
+      self._frameNext = self._frame
+
+    self._frameFraction = (frameTime - self._frame*keyFrameDuration)/(keyFrameDuration)
+    
+    
+  def computeCycleOffset(self):
+    firstFrame=0
+    lastFrame = self._mocap_data.NumFrames()-1
+    frameData = self._mocap_data._motion_data['Frames'][0]
+    frameDataNext = self._mocap_data._motion_data['Frames'][lastFrame]
+    
+    basePosStart = [frameData[1],frameData[2],frameData[3]]
+    basePosEnd = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
+    self._cycleOffset = [basePosEnd[0]-basePosStart[0],basePosEnd[1]-basePosStart[1],basePosEnd[2]-basePosStart[2]]
+    return self._cycleOffset
+    
+  def computePose(self, frameFraction):
+    frameData = self._mocap_data._motion_data['Frames'][self._frame]
+    frameDataNext = self._mocap_data._motion_data['Frames'][self._frameNext]
+    
+    self._poseInterpolator.Slerp(frameFraction, frameData, frameDataNext, self._pybullet_client)
+    #print("self._poseInterpolator.Slerp(", frameFraction,")=", pose)
+    self.computeCycleOffset()
+    oldPos = self._poseInterpolator._basePos
+    self._poseInterpolator._basePos = [oldPos[0]+self._cycleCount*self._cycleOffset[0],oldPos[1]+self._cycleCount*self._cycleOffset[1],oldPos[2]+self._cycleCount*self._cycleOffset[2]]
+    pose = self._poseInterpolator.GetPose()
+    
+    return pose
+
+
+  def convertActionToPose(self, action):
+    pose = self._poseInterpolator.ConvertFromAction(self._pybullet_client, action)
+    return pose      
+
+  def computePDForces(self, desiredPositions, desiredVelocities, maxForces):
+    if desiredVelocities==None:
+      desiredVelocities = [0]*self._totalDofs
+      
+      
+    taus = self._stablePD.computePD(bodyUniqueId=self._sim_model, 
+      jointIndices = self._jointIndicesAll,
+      desiredPositions = desiredPositions,
+      desiredVelocities = desiredVelocities,
+      kps = self._kpOrg,
+      kds = self._kdOrg,
+      maxForces = maxForces, 
+      timeStep=self._timeStep)
+    return taus
+
+  def applyPDForces(self, taus):
+    dofIndex=7
+    scaling = 1
+    for index in range (len(self._jointIndicesAll)):
+      jointIndex = self._jointIndicesAll[index]
+      if self._jointDofCounts[index]==4:
+         force=[scaling*taus[dofIndex+0],scaling*taus[dofIndex+1],scaling*taus[dofIndex+2]]
+         #print("force[", jointIndex,"]=",force)
+         self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,jointIndex,self._pybullet_client.TORQUE_CONTROL,force=force)
+      if self._jointDofCounts[index]==1:
+         force=[scaling*taus[dofIndex]]
+         #print("force[", jointIndex,"]=",force)
+         self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, jointIndex, controlMode=self._pybullet_client.TORQUE_CONTROL, force=force)
+      dofIndex+=self._jointDofCounts[index]
+      
+  def setJointMotors(self, desiredPositions, maxForces):
+    controlMode = self._pybullet_client.POSITION_CONTROL
+    startIndex=7
+    chest=1
+    neck=2
+    rightHip=3
+    rightKnee=4
+    rightAnkle=5
+    rightShoulder=6
+    rightElbow=7
+    leftHip=9
+    leftKnee=10
+    leftAnkle=11
+    leftShoulder=12
+    leftElbow=13
+    kp = 0.2
+    
+    forceScale=1
+    #self._jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
+    maxForce = [forceScale*maxForces[startIndex],forceScale*maxForces[startIndex+1],forceScale*maxForces[startIndex+2],forceScale*maxForces[startIndex+3]]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,chest,controlMode, targetPosition=self._poseInterpolator._chestRot,positionGain=kp, force=maxForce)
+    maxForce =  [maxForces[startIndex],maxForces[startIndex+1],maxForces[startIndex+2],maxForces[startIndex+3]]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,neck,controlMode,targetPosition=self._poseInterpolator._neckRot,positionGain=kp, force=maxForce)
+    maxForce =  [maxForces[startIndex],maxForces[startIndex+1],maxForces[startIndex+2],maxForces[startIndex+3]]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightHip,controlMode,targetPosition=self._poseInterpolator._rightHipRot,positionGain=kp, force=maxForce)
+    maxForce = [forceScale*maxForces[startIndex]]
+    startIndex+=1
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightKnee,controlMode,targetPosition=self._poseInterpolator._rightKneeRot,positionGain=kp, force=maxForce)
+    maxForce =  [maxForces[startIndex],maxForces[startIndex+1],maxForces[startIndex+2],maxForces[startIndex+3]]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightAnkle,controlMode,targetPosition=self._poseInterpolator._rightAnkleRot,positionGain=kp, force=maxForce)
+    maxForce = [forceScale*maxForces[startIndex],forceScale*maxForces[startIndex+1],forceScale*maxForces[startIndex+2],forceScale*maxForces[startIndex+3]]
+    startIndex+=4
+    maxForce = [forceScale*maxForces[startIndex]]
+    startIndex+=1
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightElbow, controlMode,targetPosition=self._poseInterpolator._rightElbowRot,positionGain=kp, force=maxForce)
+    maxForce =  [maxForces[startIndex],maxForces[startIndex+1],maxForces[startIndex+2],maxForces[startIndex+3]]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftHip, controlMode,targetPosition=self._poseInterpolator._leftHipRot,positionGain=kp, force=maxForce)
+    maxForce = [forceScale*maxForces[startIndex]]
+    startIndex+=1
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftKnee, controlMode,targetPosition=self._poseInterpolator._leftKneeRot,positionGain=kp, force=maxForce)
+    maxForce =  [maxForces[startIndex],maxForces[startIndex+1],maxForces[startIndex+2],maxForces[startIndex+3]]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftAnkle, controlMode,targetPosition=self._poseInterpolator._leftAnkleRot,positionGain=kp, force=maxForce)
+    maxForce =  [maxForces[startIndex],maxForces[startIndex+1],maxForces[startIndex+2],maxForces[startIndex+3]]
+    startIndex+=4
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftShoulder, controlMode,targetPosition=self._poseInterpolator._leftShoulderRot,positionGain=kp, force=maxForce)
+    maxForce = [forceScale*maxForces[startIndex]]
+    startIndex+=1
+    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftElbow, controlMode,targetPosition=self._poseInterpolator._leftElbowRot,positionGain=kp, force=maxForce)
+    #print("startIndex=",startIndex)
+      
+  def getPhase(self):
+    keyFrameDuration = self._mocap_data.KeyFrameDuraction()
+    cycleTime = keyFrameDuration*(self._mocap_data.NumFrames()-1)
+    phase = self._simTime / cycleTime
+    phase = math.fmod(phase,1.0)
+    if (phase<0):
+      phase += 1
+    return phase
+
+  def buildHeadingTrans(self, rootOrn):
+    #align root transform 'forward' with world-space x axis
+    eul = self._pybullet_client.getEulerFromQuaternion(rootOrn)
+    refDir = [1,0,0]
+    rotVec = self._pybullet_client.rotateVector(rootOrn, refDir)
+    heading = math.atan2(-rotVec[2], rotVec[0])
+    heading2=eul[1]
+    #print("heading=",heading)
+    headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0,1,0],-heading)
+    return headingOrn
+
+
+  def buildOriginTrans(self):
+    rootPos,rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._sim_model)
+    
+    #print("rootPos=",rootPos, " rootOrn=",rootOrn)
+    invRootPos=[-rootPos[0], 0, -rootPos[2]]
+    #invOrigTransPos, invOrigTransOrn = self._pybullet_client.invertTransform(rootPos,rootOrn)
+    headingOrn = self.buildHeadingTrans(rootOrn)
+    #print("headingOrn=",headingOrn)
+    headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn)
+    #print("headingMat=",headingMat)
+    #dummy, rootOrnWithoutHeading = self._pybullet_client.multiplyTransforms([0,0,0],headingOrn, [0,0,0], rootOrn)
+    #dummy, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0,0,0],rootOrnWithoutHeading, invOrigTransPos, invOrigTransOrn)
+    
+    invOrigTransPos, invOrigTransOrn = self._pybullet_client.multiplyTransforms( [0,0,0],headingOrn, invRootPos,[0,0,0,1])
+    #print("invOrigTransPos=",invOrigTransPos)
+    #print("invOrigTransOrn=",invOrigTransOrn)
+    invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn)
+    #print("invOrigTransMat =",invOrigTransMat )
+    return invOrigTransPos, invOrigTransOrn
+
+  def getState(self):
+
+    stateVector = []
+    phase = self.getPhase()
+    #print("phase=",phase)
+    stateVector.append(phase)
+    
+    rootTransPos, rootTransOrn=self.buildOriginTrans()
+    basePos,baseOrn = self._pybullet_client.getBasePositionAndOrientation(self._sim_model)
+    
+    rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, basePos,[0,0,0,1])
+    #print("!!!rootPosRel =",rootPosRel )
+    #print("rootTransPos=",rootTransPos)
+    #print("basePos=",basePos)
+    localPos,localOrn = self._pybullet_client.multiplyTransforms( rootTransPos, rootTransOrn , basePos,baseOrn )
+    
+    localPos=[localPos[0]-rootPosRel[0],localPos[1]-rootPosRel[1],localPos[2]-rootPosRel[2]]
+    #print("localPos=",localPos)
+        
+    stateVector.append(rootPosRel[1])
+    
+    #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]
+    
+    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)
+      linkPos = ls[0]
+      linkOrn = ls[1]
+      linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, linkPos,linkOrn)
+      if (linkOrnLocal[3]<0):
+        linkOrnLocal=[-linkOrnLocal[0],-linkOrnLocal[1],-linkOrnLocal[2],-linkOrnLocal[3]]
+      linkPosLocal=[linkPosLocal[0]-rootPosRel[0],linkPosLocal[1]-rootPosRel[1],linkPosLocal[2]-rootPosRel[2]]
+      for l in linkPosLocal:
+        stateVector.append(l)
+      #re-order the quaternion, DeepMimic uses w,x,y,z
+      
+      if (linkOrnLocal[3]<0):
+        linkOrnLocal[0]*=-1
+        linkOrnLocal[1]*=-1
+        linkOrnLocal[2]*=-1
+        linkOrnLocal[3]*=-1
+        
+      stateVector.append(linkOrnLocal[3])
+      stateVector.append(linkOrnLocal[0])
+      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)
+      linkLinVel = ls[6]
+      linkAngVel = ls[7]
+      linkLinVelLocal  , unused = self._pybullet_client.multiplyTransforms([0,0,0], rootTransOrn, linkLinVel,[0,0,0,1])
+      #linkLinVelLocal=[linkLinVelLocal[0]-rootPosRel[0],linkLinVelLocal[1]-rootPosRel[1],linkLinVelLocal[2]-rootPosRel[2]]
+      linkAngVelLocal ,unused = self._pybullet_client.multiplyTransforms([0,0,0], rootTransOrn, linkAngVel,[0,0,0,1])
+      
+      for l in linkLinVelLocal:
+        stateVector.append(l)
+      for l in linkAngVelLocal:
+        stateVector.append(l)
+    
+    #print("stateVector len=",len(stateVector))  
+    #for st in range (len(stateVector)):
+    #  print("state[",st,"]=",stateVector[st])
+    return stateVector
+  
+  def terminates(self):
+    #check if any non-allowed body part hits the ground
+    terminates=False
+    pts = self._pybullet_client.getContactPoints()
+    for p in pts:
+      part = -1
+      #ignore self-collision
+      if (p[1]==p[2]):
+        continue
+      if (p[1]==self._sim_model):
+        part=p[3]
+      if (p[2]==self._sim_model):
+        part=p[4]
+      if (part >=0 and part not in self._allowed_body_parts):
+        #print("terminating part:", part)
+        terminates=True
+      
+    return terminates
+    
+  def quatMul(self, q1, q2):
+      return [ q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1],
+               q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2],
+              q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0],
+              q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]]
+              
+  def calcRootAngVelErr(self, vel0, vel1):
+    diff = [vel0[0]-vel1[0],vel0[1]-vel1[1], vel0[2]-vel1[2]]
+    return diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
+
+  
+    
+  def calcRootRotDiff(self,orn0, orn1):
+    orn0Conj = [-orn0[0],-orn0[1],-orn0[2],orn0[3]]
+    q_diff = self.quatMul(orn1, orn0Conj)
+    axis,angle = self._pybullet_client.getAxisAngleFromQuaternion(q_diff)
+    return angle*angle
+  
+  def getReward(self, pose):
+    #from DeepMimic double cSceneImitate::CalcRewardImitate
+    #todo: compensate for ground height in some parts, once we move to non-flat terrain
+    pose_w = 0.5
+    vel_w = 0.05
+    end_eff_w = 0.15
+    root_w = 0.2
+    com_w = 0 #0.1
+
+    total_w = pose_w + vel_w + end_eff_w + root_w + com_w
+    pose_w /= total_w
+    vel_w /= total_w
+    end_eff_w /= total_w
+    root_w /= total_w
+    com_w /= total_w
+
+    pose_scale = 2
+    vel_scale = 0.1
+    end_eff_scale = 40
+    root_scale = 5
+    com_scale = 10
+    err_scale = 1
+
+    reward = 0
+
+    pose_err = 0
+    vel_err = 0
+    end_eff_err = 0
+    root_err = 0
+    com_err = 0
+    heading_err = 0
+    
+    #create a mimic reward, comparing the dynamics humanoid with a kinematic one
+    
+    #pose = self.InitializePoseFromMotionData()
+    #print("self._kin_model=",self._kin_model)
+    #print("kinematicHumanoid #joints=",self._pybullet_client.getNumJoints(self._kin_model))
+    #self.ApplyPose(pose, True, True, self._kin_model, self._pybullet_client)
+
+    #const Eigen::VectorXd& pose0 = sim_char.GetPose();
+    #const Eigen::VectorXd& vel0 = sim_char.GetVel();
+    #const Eigen::VectorXd& pose1 = kin_char.GetPose();
+    #const Eigen::VectorXd& vel1 = kin_char.GetVel();
+    #tMatrix origin_trans = sim_char.BuildOriginTrans();
+    #tMatrix kin_origin_trans = kin_char.BuildOriginTrans();
+    #
+    #tVector com0_world = sim_char.CalcCOM();
+    #tVector com_vel0_world = sim_char.CalcCOMVel();
+    #tVector com1_world;
+    #tVector com_vel1_world;
+    #cRBDUtil::CalcCoM(joint_mat, body_defs, pose1, vel1, com1_world, com_vel1_world);
+    #
+    root_id = 0
+    #tVector root_pos0 = cKinTree::GetRootPos(joint_mat, pose0);
+    #tVector root_pos1 = cKinTree::GetRootPos(joint_mat, pose1);
+    #tQuaternion root_rot0 = cKinTree::GetRootRot(joint_mat, pose0);
+    #tQuaternion root_rot1 = cKinTree::GetRootRot(joint_mat, pose1);
+    #tVector root_vel0 = cKinTree::GetRootVel(joint_mat, vel0);
+    #tVector root_vel1 = cKinTree::GetRootVel(joint_mat, vel1);
+    #tVector root_ang_vel0 = cKinTree::GetRootAngVel(joint_mat, vel0);
+    #tVector root_ang_vel1 = cKinTree::GetRootAngVel(joint_mat, vel1);
+
+    mJointWeights = [0.20833,0.10416, 0.0625, 0.10416,
+      0.0625, 0.041666666666666671, 0.0625, 0.0416,
+      0.00, 0.10416,  0.0625, 0.0416, 0.0625, 0.0416, 0.0000]
+      
+    num_end_effs = 0
+    num_joints = 15
+    
+    root_rot_w = mJointWeights[root_id]
+    rootPosSim,rootOrnSim = self._pybullet_client.getBasePositionAndOrientation(self._sim_model)
+    rootPosKin ,rootOrnKin = self._pybullet_client.getBasePositionAndOrientation(self._kin_model)
+    linVelSim, angVelSim = self._pybullet_client.getBaseVelocity(self._sim_model)
+    linVelKin, angVelKin = self._pybullet_client.getBaseVelocity(self._kin_model)
+    
+    
+    root_rot_err = self.calcRootRotDiff(rootOrnSim,rootOrnKin)
+    pose_err += root_rot_w * root_rot_err
+    
+    
+    root_vel_diff  = [linVelSim[0]-linVelKin[0],linVelSim[1]-linVelKin[1],linVelSim[2]-linVelKin[2]]
+    root_vel_err = root_vel_diff[0]*root_vel_diff[0]+root_vel_diff[1]*root_vel_diff[1]+root_vel_diff[2]*root_vel_diff[2]
+    
+    root_ang_vel_err = self.calcRootAngVelErr( angVelSim, angVelKin)
+    vel_err += root_rot_w * root_ang_vel_err
+
+    for j in range (num_joints):
+      curr_pose_err = 0
+      curr_vel_err = 0
+      w = mJointWeights[j];
+      
+      simJointInfo = self._pybullet_client.getJointStateMultiDof(self._sim_model, j)
+      
+      #print("simJointInfo.pos=",simJointInfo[0])
+      #print("simJointInfo.vel=",simJointInfo[1])
+      kinJointInfo = self._pybullet_client.getJointStateMultiDof(self._kin_model,j)
+      #print("kinJointInfo.pos=",kinJointInfo[0])
+      #print("kinJointInfo.vel=",kinJointInfo[1])
+      if (len(simJointInfo[0])==1):
+        angle = simJointInfo[0][0]-kinJointInfo[0][0]
+        curr_pose_err = angle*angle
+        velDiff = simJointInfo[1][0]-kinJointInfo[1][0]
+        curr_vel_err = velDiff*velDiff
+      if (len(simJointInfo[0])==4):
+        #print("quaternion diff")
+        diffQuat = self._pybullet_client.getDifferenceQuaternion(simJointInfo[0],kinJointInfo[0])
+        axis,angle = self._pybullet_client.getAxisAngleFromQuaternion(diffQuat)
+        curr_pose_err = angle*angle
+        diffVel = [simJointInfo[1][0]-kinJointInfo[1][0],simJointInfo[1][1]-kinJointInfo[1][1],simJointInfo[1][2]-kinJointInfo[1][2]]
+        curr_vel_err = diffVel[0]*diffVel[0]+diffVel[1]*diffVel[1]+diffVel[2]*diffVel[2]
+      
+      
+      pose_err += w * curr_pose_err
+      vel_err += w * curr_vel_err
+    
+      is_end_eff = j in self._end_effectors
+      if is_end_eff:
+        
+        linkStateSim = self._pybullet_client.getLinkState(self._sim_model, j)
+        linkStateKin = self._pybullet_client.getLinkState(self._kin_model, j)
+        linkPosSim = linkStateSim[0]
+        linkPosKin = linkStateKin[0]
+        linkPosDiff = [linkPosSim[0]-linkPosKin[0],linkPosSim[1]-linkPosKin[1],linkPosSim[2]-linkPosKin[2]]
+        curr_end_err = linkPosDiff[0]*linkPosDiff[0]+linkPosDiff[1]*linkPosDiff[1]+linkPosDiff[2]*linkPosDiff[2]
+        end_eff_err += curr_end_err
+        num_end_effs+=1
+    
+    if (num_end_effs > 0):
+      end_eff_err /= num_end_effs
+    
+    #double root_ground_h0 = mGround->SampleHeight(sim_char.GetRootPos())
+    #double root_ground_h1 = kin_char.GetOriginPos()[1]
+    #root_pos0[1] -= root_ground_h0
+    #root_pos1[1] -= root_ground_h1
+    root_pos_diff = [rootPosSim[0]-rootPosKin[0],rootPosSim[1]-rootPosKin[1],rootPosSim[2]-rootPosKin[2]]
+    root_pos_err = root_pos_diff[0]*root_pos_diff[0]+root_pos_diff[1]*root_pos_diff[1]+root_pos_diff[2]*root_pos_diff[2]
+    #  
+    #root_rot_err = cMathUtil::QuatDiffTheta(root_rot0, root_rot1)
+    #root_rot_err *= root_rot_err
+
+    #root_vel_err = (root_vel1 - root_vel0).squaredNorm()
+    #root_ang_vel_err = (root_ang_vel1 - root_ang_vel0).squaredNorm()
+
+    root_err = root_pos_err + 0.1 * root_rot_err+ 0.01 * root_vel_err+ 0.001 * root_ang_vel_err
+    
+    #com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm()
+    
+    #print("pose_err=",pose_err)
+    #print("vel_err=",vel_err)
+    pose_reward = math.exp(-err_scale * pose_scale * pose_err)
+    vel_reward = math.exp(-err_scale * vel_scale * vel_err)
+    end_eff_reward = math.exp(-err_scale * end_eff_scale * end_eff_err)
+    root_reward = math.exp(-err_scale * root_scale * root_err)
+    com_reward = math.exp(-err_scale * com_scale * com_err)
+
+    reward = pose_w * pose_reward + vel_w * vel_reward + end_eff_w * end_eff_reward + root_w * root_reward + com_w * com_reward
+
+    
+    # pose_reward,vel_reward,end_eff_reward, root_reward, com_reward);
+    #print("reward=",reward)
+    #print("pose_reward=",pose_reward)
+    #print("vel_reward=",vel_reward)
+    #print("end_eff_reward=",end_eff_reward)
+    #print("root_reward=",root_reward)
+    #print("com_reward=",com_reward)
+    
+    return reward

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

@@ -1,15 +1,19 @@
-import pybullet as p
+import pybullet as p1
+from pybullet_utils import bullet_client
 import pybullet_data
+from pybullet_utils import pd_controller_stable
 
 import time
 import motion_capture_data
 import quadrupedPoseInterpolator
 
-p.connect(p.GUI)
+useConstraints = False
+  
+p =  bullet_client.BulletClient(connection_mode=p1.GUI)
 p.setAdditionalSearchPath(pybullet_data.getDataPath())
 
 plane = p.loadURDF("plane.urdf")
-p.setGravity(0,0,-9.8)
+p.setGravity(0,0,-10)
 timeStep=1./500
 p.setTimeStep(timeStep)
 #p.setDefaultContactERP(0)
@@ -21,7 +25,10 @@ startPos=[0.007058990464444105, 0.03149299192130908, 0.4918981912395484]
 startOrn=[0.005934649695708604, 0.7065453990917289, 0.7076373820553712, -0.0027774940359030264]
 quadruped = p.loadURDF("laikago/laikago.urdf",startPos,startOrn, flags = urdfFlags,useFixedBase=False)
 p.resetBasePositionAndOrientation(quadruped,startPos,startOrn)
-
+if not useConstraints:
+  for j in range(p.getNumJoints(quadruped)):
+    p.setJointMotorControl2(quadruped,j,p.POSITION_CONTROL,force=0)
+  
 #This cube is added as a soft constraint to keep the laikago from falling 
 #since we didn't train it yet, it doesn't balance
 cube = p.loadURDF("cube_no_rotation.urdf",[0,0,-0.5],[0,0.5,0.5,0])
@@ -117,6 +124,7 @@ print("mocapData.KeyFrameDuraction=",mocapData.KeyFrameDuraction())
 print("mocapData.getCycleTime=",mocapData.getCycleTime())
 print("mocapData.computeCycleOffset=",mocapData.computeCycleOffset())
 
+stablePD = pd_controller_stable.PDControllerStable(p)
 
 cycleTime = mocapData.getCycleTime()
 t=0
@@ -146,12 +154,50 @@ while t<10.*cycleTime:
   frameData = mocapData._motion_data['Frames'][frame]
   frameDataNext = mocapData._motion_data['Frames'][frameNext]
     
-  joints,qdot=qpi.Slerp(frameFraction, frameData, frameDataNext, p)
+  jointsStr,qdot=qpi.Slerp(frameFraction, frameData, frameDataNext, p)
   
   maxForce = p.readUserDebugParameter(maxForceId)
-  for j in range (12):
+  print("jointIds=",jointIds)
-    targetPos = float(joints[j])
+
-    p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
+  if useConstraints:
+    for j in range (12):
+      #skip the base positional dofs
+      targetPos = float(jointsStr[j+7])
+      p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
+    
+  else:
+    desiredPositions=[]
+    for j in range (7):
+      targetPosUnmodified = float(jointsStr[j])
+      desiredPositions.append(targetPosUnmodified)
+    for j in range (12):
+      targetPosUnmodified = float(jointsStr[j+7])
+      targetPos=jointDirections[j]*targetPosUnmodified+jointOffsets[j]
+      desiredPositions.append(targetPos)
+    numBaseDofs=6
+    totalDofs=12+numBaseDofs
+    desiredVelocities=None
+    if desiredVelocities==None:
+      desiredVelocities = [0]*totalDofs 
+    taus = stablePD.computePD(bodyUniqueId=quadruped, 
+      jointIndices = jointIds,
+      desiredPositions = desiredPositions,
+      desiredVelocities = desiredVelocities,
+      kps = [4000]*totalDofs,
+      kds = [40]*totalDofs,
+      maxForces = [500]*totalDofs, 
+      timeStep=timeStep)
+    
+    dofIndex=6
+    scaling = 1
+    for index in range (len(jointIds)):
+      jointIndex = jointIds[index]
+      force=[scaling*taus[dofIndex]]
+      print("force[", jointIndex,"]=",force)
+      p.setJointMotorControlMultiDof(quadruped, jointIndex, controlMode=p.TORQUE_CONTROL, force=force)
+      dofIndex+=1
+   
+
   p.stepSimulation()
   t+=timeStep
   time.sleep(timeStep)

examples/pybullet/gym/pybullet_utils/pd_controller_stable.py

@@ -165,11 +165,25 @@ class PDControllerStable(object):
       self._pb = pb
 
     def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
-      numJoints = self._pb.getNumJoints(bodyUniqueId)
+      numBaseDofs = 0
-      jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+      numPosBaseDofs=0
+      baseMass = self._pb.getDynamicsInfo(bodyUniqueId,-1)[0]
+      curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
       q1 = []
       qdot1 = []
       zeroAccelerations = []
+      qError=[]
+      if (baseMass>0):
+        numBaseDofs=6
+        numPosBaseDofs=7
+        q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
+        qdot1=[0]*numBaseDofs
+        zeroAccelerations = [0]*numBaseDofs
+        angDiff=[0,0,0]
+        qError=[ desiredPositions[0]-curPos[0], desiredPositions[1]-curPos[1], desiredPositions[2]-curPos[2],angDiff[0],angDiff[1],angDiff[2]]
+      numJoints = self._pb.getNumJoints(bodyUniqueId)
+      jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+      
       for i in range (numJoints):
         q1.append(jointStates[i][0])
         qdot1.append(jointStates[i][1])
@@ -178,7 +192,10 @@ class PDControllerStable(object):
       qdot=np.array(qdot1)
       qdes = np.array(desiredPositions)
       qdotdes = np.array(desiredVelocities)
-      qError = qdes - q
+      #qError = qdes - q
+      for j in range(numJoints):
+        qError.append(desiredPositions[j+numPosBaseDofs]-q1[j+numPosBaseDofs])
+      #print("qError=",qError)
       qdotError = qdotdes - qdot
       Kp = np.diagflat(kps)
       Kd = np.diagflat(kds)

examples/pybullet/pybullet.c

@@ -330,6 +330,28 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObjec
 			statusHandle = b3SubmitClientCommandAndWaitStatus(
 				sm, b3InitStepSimulationCommand(sm));
 			statusType = b3GetStatusType(statusHandle);
+
+			if (statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED)
+			{
+				struct b3ForwardDynamicsAnalyticsArgs analyticsData;
+				int numIslands = 0;
+				int i;
+				numIslands = b3GetStatusForwardDynamicsAnalyticsData(statusHandle, &analyticsData);
+
+				PyObject* pyAnalyticsData = PyTuple_New(numIslands);
+				for (i=0;i<numIslands;i++)
+				{
+					int numFields = 4;
+					PyObject* pyIslandData = PyTuple_New(numFields);
+					PyTuple_SetItem(pyIslandData, 0, PyLong_FromLong(analyticsData.m_islandData[i].m_islandId));
+					PyTuple_SetItem(pyIslandData, 1, PyLong_FromLong(analyticsData.m_islandData[i].m_numBodies));
+					PyTuple_SetItem(pyIslandData, 2, PyLong_FromLong(analyticsData.m_islandData[i].m_numIterationsUsed));
+					PyTuple_SetItem(pyIslandData, 3, PyFloat_FromDouble(analyticsData.m_islandData[i].m_remainingLeastSquaresResidual));
+					PyTuple_SetItem(pyAnalyticsData, i, pyIslandData);
+				}
+
+				return pyAnalyticsData;
+			}
 		}
 	}
 
@@ -1545,8 +1567,9 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 	double globalCFM = -1;
 
 	int minimumSolverIslandSize = -1;
-
+	int reportSolverAnalytics = -1;
 	int physicsClientId = 0;
+
 	static char* kwlist[] = {"fixedTimeStep",
 							 "numSolverIterations",
 							 "useSplitImpulse",
@@ -1570,10 +1593,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 							 "constraintSolverType",
 							 "globalCFM",
 							 "minimumSolverIslandSize",
+							 "reportSolverAnalytics",
 							 "physicsClientId", NULL};
 
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
+	if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddiididdiidiii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
-									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId))
+									 &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, 
+									&reportSolverAnalytics, &physicsClientId))
 	{
 		return NULL;
 	}
@@ -1690,6 +1715,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 		{
 			b3PhysicsParamSetDefaultGlobalCFM(command, globalCFM);
 		}
+		if (reportSolverAnalytics >= 0)
+		{
+			b3PhysicsParamSetSolverAnalytics(command, reportSolverAnalytics);
+		}
 		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
 	}
 

src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h

@@ -64,6 +64,7 @@ struct btContactSolverInfoData
 	btScalar m_restitutionVelocityThreshold;
 	bool m_jointFeedbackInWorldSpace;
 	bool m_jointFeedbackInJointFrame;
+	int m_reportSolverAnalytics;
 };
 
 struct btContactSolverInfo : public btContactSolverInfoData
@@ -98,6 +99,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_restitutionVelocityThreshold = 0.2f;  //if the relative velocity is below this threshold, there is zero restitution
 		m_jointFeedbackInWorldSpace = false;
 		m_jointFeedbackInJointFrame = false;
+		m_reportSolverAnalytics = 0;
 	}
 };
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -2239,6 +2239,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 #ifdef VERBOSE_RESIDUAL_PRINTF
 				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
 #endif
+				m_analyticsData.m_numSolverCalls++;
+				m_analyticsData.m_numIterationsUsed = iteration+1;
+				m_analyticsData.m_islandId = -2;
+				if (numBodies>0)
+					m_analyticsData.m_islandId = bodies[0]->getCompanionId();
+				m_analyticsData.m_numBodies = numBodies;
+				m_analyticsData.m_numContactManifolds = numManifolds;
+				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
 				break;
 			}
 		}

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h

@@ -91,10 +91,29 @@ struct btSISolverSingleIterationData
 	}
 };
 
+struct btSolverAnalyticsData
+{
+	btSolverAnalyticsData()
+	{
+		m_numSolverCalls = 0;
+		m_numIterationsUsed = -1;
+		m_remainingLeastSquaresResidual = -1;
+		m_islandId = -2;
+	}
+	int m_islandId;
+	int m_numBodies;
+	int m_numContactManifolds;
+	int m_numSolverCalls;
+	int m_numIterationsUsed;
+	double m_remainingLeastSquaresResidual;
+};
+
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 ATTRIBUTE_ALIGNED16(class)
 btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
+	
+
 protected:
 	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
 	btConstraintArray m_tmpSolverContactConstraintPool;
@@ -283,6 +302,8 @@ public:
 		m_resolveSingleConstraintRowLowerLimit = rowSolver;
 	}
 
+
+
 	///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
 	static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
 	static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
@@ -296,6 +317,7 @@ public:
 	static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
 	static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
 
+	btSolverAnalyticsData m_analyticsData;
 };
 
 #endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -207,6 +207,7 @@ public:
 	}
 };
 
+
 struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 {
 	btContactSolverInfo* m_solverInfo;
@@ -224,6 +225,8 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
 
+	btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
+
 	MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
 										 btDispatcher* dispatcher)
 		: m_solverInfo(NULL),
@@ -244,6 +247,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 
 	SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
 	{
+		m_islandAnalyticsData.clear();
 		btAssert(solverInfo);
 		m_solverInfo = solverInfo;
 
@@ -270,6 +274,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		{
 			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
 			m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+			if (m_solverInfo->m_reportSolverAnalytics&1)
+			{
+				m_solver->m_analyticsData.m_islandId = islandId;
+				m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+			}
 		}
 		else
 		{
@@ -335,7 +344,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 
 				if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
 				{
-					processConstraints();
+					processConstraints(islandId);
 				}
 				else
 				{
@@ -344,7 +353,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 			}
 		}
 	}
-	void processConstraints()
+	void processConstraints(int islandId=-1)
 	{
 		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
 		btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
@@ -354,6 +363,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
 
 		m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
+		if (m_bodies.size() && (m_solverInfo->m_reportSolverAnalytics&1))
+		{
+			m_solver->m_analyticsData.m_islandId = islandId;
+			m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+		}
 		m_bodies.resize(0);
 		m_manifolds.resize(0);
 		m_constraints.resize(0);
@@ -361,6 +375,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 	}
 };
 
+void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
+{
+	islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
+}
+
 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
 	: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
 	  m_multiBodyConstraintSolver(constraintSolver)
@@ -721,10 +740,13 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 				{
 					if (!bod->isUsingRK4Integration())
 					{
-						bool isConstraintPass = true;
+						if (bod->internalNeedsJointFeedback())
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+						{
-						getSolverInfo().m_jointFeedbackInWorldSpace,
+							bool isConstraintPass = true;
-						getSolverInfo().m_jointFeedbackInJointFrame);
+							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+								getSolverInfo().m_jointFeedbackInWorldSpace,
+								getSolverInfo().m_jointFeedbackInJointFrame);
+						}
 					}
 				}
 			}

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -109,5 +109,7 @@ public:
 	virtual void serialize(btSerializer* serializer);
 	virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
 	virtual void setConstraintSolver(btConstraintSolver* solver);
+	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
+
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H