diff options
author | erwincoumans <erwin.coumans@gmail.com> | 2019-04-15 08:52:03 -0700 |
---|---|---|
committer | GitHub <noreply@github.com> | 2019-04-15 08:52:03 -0700 |
commit | a929e8f2e5bead97dd76e58e155a7078db7207d0 (patch) | |
tree | 708da965c03c4c3574c3d514dcafdb13ac34c4e5 | |
parent | c6a43e0a5b1e77ab323a5905368e0963cd10bfaf (diff) | |
parent | e97a7d77af352225c86f5ae7fd6f106c4f295e42 (diff) | |
download | bullet3-a929e8f2e5bead97dd76e58e155a7078db7207d0.tar.gz |
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
16 files changed, 857 insertions, 52 deletions
diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1e515b576..920a463b7 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 9682486ec..290c8b36b 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index da65be89c..b0c6697ec 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5be07b05a..1d70a9df0 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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_NUM_SOLVER_ITERATIONS = 4, - SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS = 8, - SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16, - SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP = 32, - SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS = 64, - SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE = 128, - SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256, - SIM_PARAM_UPDATE_COLLISION_FILTER_MODE = 512, - SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024, - SIM_PARAM_ENABLE_CONE_FRICTION = 2048, - SIM_PARAM_ENABLE_FILE_CACHING = 4096, - SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192, - SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP = 16384, - SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768, - SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536, - SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072, - SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 262144, - SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM = 524288, - SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1048576, - SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 2097152, - SIM_PARAM_UPDATE_CONTACT_SLOP = 4194304, - SIM_PARAM_ENABLE_SAT = 8388608, - SIM_PARAM_CONSTRAINT_SOLVER_TYPE = 16777216, - SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 33554432, + SIM_PARAM_UPDATE_GRAVITY = 1<<1, + SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS = 1<<2, + SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS = 1<<3, + SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 1<<4, + SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP = 1<<5, + SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS = 1<<6, + SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE = 1<<7, + SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 1<<8, + SIM_PARAM_UPDATE_COLLISION_FILTER_MODE = 1 << 9, + SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1 << 10, + SIM_PARAM_ENABLE_CONE_FRICTION = 1 << 11, + SIM_PARAM_ENABLE_FILE_CACHING = 1 << 12, + SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 1 << 13, + SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP = 1 << 14, + SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 1 << 15, + SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 1 << 16, + SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 1 << 17, + SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 1 << 18, + SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM = 1 << 19, + SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1 << 20, + SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD = 1 << 21, + SIM_PARAM_UPDATE_CONTACT_SLOP = 1 << 22, + SIM_PARAM_ENABLE_SAT = 1 << 23, + SIM_PARAM_CONSTRAINT_SOLVER_TYPE = 1 << 24, + 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; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index a569cbaf0..b2c2f6c68 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/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 diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index c786c737a..0f60543ba 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/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); diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadrupedPoseInterpolator.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadrupedPoseInterpolator.py index 61a1f5824..9aa6d2652 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadrupedPoseInterpolator.py +++ b/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=[] - jointVelocities=[] + jointPositions=[self._basePos[0],self._basePos[1],self._basePos[2], + 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] diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadruped_stable_pd.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadruped_stable_pd.py new file mode 100644 index 000000000..ce66fb462 --- /dev/null +++ b/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 diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py index a429bad5b..1ad8972ed 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py +++ b/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): - targetPos = float(joints[j]) - p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce) + print("jointIds=",jointIds) + + 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) diff --git a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py index 04424b9a1..2171b34bd 100644 --- a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py +++ b/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) - jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices) + numBaseDofs = 0 + 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) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0df3f14da..9946da3cf 100644 --- a/examples/pybullet/pybullet.c +++ b/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, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &deterministicOverlappingPairs, &allowedCcdPenetration, &jointFeedbackMode, &solverResidualThreshold, &contactSlop, &enableSAT, &constraintSolverType, &globalCFM, &minimumSolverIslandSize, &physicsClientId)) + 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, + &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); } diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 89f8db8b1..63d7c98e1 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/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; } }; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 9f76713db..d3b71e458 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/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; } } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index aca1d0988..542ea23c9 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/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
\ No newline at end of file diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index faaa211f6..2050baa75 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/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; - bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, - getSolverInfo().m_jointFeedbackInWorldSpace, - getSolverInfo().m_jointFeedbackInJointFrame); + if (bod->internalNeedsJointFeedback()) + { + bool isConstraintPass = true; + bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass, + getSolverInfo().m_jointFeedbackInWorldSpace, + getSolverInfo().m_jointFeedbackInJointFrame); + } } } } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 641238f3b..e36c2f7aa 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/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 |