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
This commit is contained in:
erwincoumans
GitHub
@@ -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;
|
||||
|
||||
@@ -349,6 +349,7 @@ 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);
|
||||
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -1792,6 +1792,7 @@ void CMainApplication::RenderStereoTargets()
|
||||
|
||||
//m_app->drawGrid(gridUp);
|
||||
|
||||
m_app->m_instancingRenderer->setRenderFrameBuffer(0);
|
||||
glBindFramebuffer(GL_FRAMEBUFFER, 0);
|
||||
|
||||
glDisable(GL_MULTISAMPLE);
|
||||
|
||||
@@ -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]
|
||||
|
||||
577
examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadruped_stable_pd.py
vendored
Normal file
577
examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadruped_stable_pd.py
vendored
Normal file
@@ -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
|
||||
@@ -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,6 +25,9 @@ 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
|
||||
@@ -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)
|
||||
print("jointIds=",jointIds)
|
||||
|
||||
if useConstraints:
|
||||
for j in range (12):
|
||||
targetPos = float(joints[j])
|
||||
#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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -720,6 +739,8 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
if (bod->internalNeedsJointFeedback())
|
||||
{
|
||||
if (!bod->isUsingRK4Integration())
|
||||
{
|
||||
if (bod->internalNeedsJointFeedback())
|
||||
{
|
||||
bool isConstraintPass = true;
|
||||
bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
|
||||
@@ -730,6 +751,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < this->m_multiBodies.size(); i++)
|
||||
{
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user