mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 05:40:05 +00:00
erwincoumans
GitHub
commit
fd161fa061
@ -5722,7 +5722,7 @@ bool PhysicsServerCommandProcessor::processCollisionFilterCommand(const struct S
|
||||
btCollisionObject* colObj = 0;
|
||||
if (body->m_multiBody)
|
||||
{
|
||||
if (clientCmd.m_collisionFilterArgs.m_linkIndexA)
|
||||
if (clientCmd.m_collisionFilterArgs.m_linkIndexA==-1)
|
||||
{
|
||||
colObj = body->m_multiBody->getBaseCollider();
|
||||
}
|
||||
@ -7653,6 +7653,15 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
|
||||
{
|
||||
mb->setCanSleep(false);
|
||||
}
|
||||
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableWakeup)
|
||||
{
|
||||
mb->setCanWakeup(true);
|
||||
}
|
||||
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableWakeup)
|
||||
{
|
||||
mb->setCanWakeup(false);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
|
||||
|
||||
@ -326,6 +326,8 @@ enum DynamicsActivationState
|
||||
eActivationStateDisableSleeping = 2,
|
||||
eActivationStateWakeUp = 4,
|
||||
eActivationStateSleep = 8,
|
||||
eActivationStateEnableWakeup = 16,
|
||||
eActivationStateDisableWakeup = 32,
|
||||
};
|
||||
|
||||
struct b3DynamicsInfo
|
||||
@ -844,6 +846,7 @@ enum eURDF_Flags
|
||||
URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
|
||||
URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
|
||||
URDF_MAINTAIN_LINK_ORDER = 131072,
|
||||
URDF_ENABLE_WAKEUP = 262144,
|
||||
};
|
||||
|
||||
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes
|
||||
|
||||
@ -1839,10 +1839,13 @@ bool PhysXServerCommandProcessor::processForwardDynamicsCommand(const struct Sha
|
||||
float* depthBuffer = 0;
|
||||
int* segmentationMaskBuffer = 0;
|
||||
int startPixelIndex = 0;
|
||||
|
||||
int width = 1024;
|
||||
int height = 768;
|
||||
m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(width, height);
|
||||
int numPixelsCopied = 0;
|
||||
|
||||
|
||||
m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels,
|
||||
depthBuffer, numRequestedPixels,
|
||||
segmentationMaskBuffer, numRequestedPixels,
|
||||
|
||||
@ -25,7 +25,7 @@
|
||||
#include "../../CommonInterfaces/CommonFileIOInterface.h"
|
||||
#include "../../Importers/ImportObjDemo/LoadMeshFromObj.h"
|
||||
#include "../../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
|
||||
#include "Importers/ImportURDFDemo/UrdfParser.h"
|
||||
#include "../../Importers/ImportURDFDemo/UrdfParser.h"
|
||||
|
||||
|
||||
|
||||
|
||||
@ -2,7 +2,7 @@
|
||||
#define URDF2PHYSX_H
|
||||
|
||||
#include "Bullet3Common/b3AlignedObjectArray.h"
|
||||
#include "Importers/ImportURDFDemo/URDFJointTypes.h"
|
||||
#include "../../Importers/ImportURDFDemo/URDFJointTypes.h"
|
||||
|
||||
namespace physx
|
||||
{
|
||||
|
||||
@ -241,8 +241,10 @@ static void SimpleResizeCallback(float widthf, float heightf)
|
||||
if (gWindow && gWindow->m_data->m_instancingRenderer)
|
||||
{
|
||||
gWindow->m_data->m_instancingRenderer->resize(width, height);
|
||||
gWindow->setWidthAndHeight(width, height);
|
||||
}
|
||||
|
||||
|
||||
//if (gApp && gApp->m_instancingRenderer)
|
||||
// gApp->m_instancingRenderer->resize(width, height);
|
||||
//
|
||||
@ -1281,7 +1283,7 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
|
||||
{
|
||||
m_data->m_window->endRendering();
|
||||
m_data->m_window->startRendering();
|
||||
glViewport(0,0, sourceWidth, sourceHeight);
|
||||
glViewport(0,0, sourceWidth*m_data->m_window->getRetinaScale(), sourceHeight*m_data->m_window->getRetinaScale());
|
||||
B3_PROFILE("m_instancingRenderer render");
|
||||
m_data->m_instancingRenderer->writeTransforms();
|
||||
if (m_data->m_hasLightDirection)
|
||||
@ -1373,7 +1375,7 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
|
||||
{
|
||||
{
|
||||
m_data->m_window->startRendering();
|
||||
glViewport(0,0, sourceWidth, sourceHeight);
|
||||
glViewport(0,0, sourceWidth*m_data->m_window->getRetinaScale(), sourceHeight*m_data->m_window->getRetinaScale());
|
||||
BT_PROFILE("renderScene");
|
||||
m_data->m_instancingRenderer->renderSceneInternal(B3_SEGMENTATION_MASK_RENDERMODE);
|
||||
}
|
||||
|
||||
@ -9,7 +9,7 @@ if (cid<0):
|
||||
|
||||
p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024)
|
||||
p.setTimeStep(1./120.)
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json")
|
||||
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json")
|
||||
#useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone)
|
||||
p.loadURDF("plane100.urdf", useMaximalCoordinates=True)
|
||||
#disable rendering during creation.
|
||||
@ -112,9 +112,6 @@ indices=[ 0, 1, 2, 0, 2, 3, #//ground face
|
||||
#p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
|
||||
#the visual shape and collision shape can be re-used by all createMultiBody instances (instancing)
|
||||
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals)
|
||||
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices)
|
||||
|
||||
#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj")
|
||||
collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale)
|
||||
|
||||
texUid = p.loadTexture("tex256.png")
|
||||
@ -124,16 +121,16 @@ batchPositions=[]
|
||||
|
||||
|
||||
|
||||
for x in range (33):
|
||||
for y in range (34):
|
||||
for z in range (4):
|
||||
for x in range (32):
|
||||
for y in range (32):
|
||||
for z in range (10):
|
||||
batchPositions.append([x*meshScale[0]*5.5,y*meshScale[1]*5.5,(0.5+z)*meshScale[2]*2.5])
|
||||
|
||||
bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True)
|
||||
bodyUid = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True)
|
||||
p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid)
|
||||
|
||||
#p.syncBodyInfo()
|
||||
#print("numBodies=",p.getNumBodies())
|
||||
p.syncBodyInfo()
|
||||
print("numBodies=",p.getNumBodies())
|
||||
p.stopStateLogging(logId)
|
||||
p.setGravity(0,0,-10)
|
||||
|
||||
|
||||
@ -38,9 +38,9 @@ timeStep = 1./600.
|
||||
|
||||
p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
|
||||
|
||||
path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
#path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt"
|
||||
#path = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
|
||||
path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt"
|
||||
#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
|
||||
#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
|
||||
|
||||
|
||||
#p.loadURDF("plane.urdf",[0,0,-1.03])
|
||||
|
||||
@ -26,8 +26,15 @@ class HumanoidStablePD(object):
|
||||
self._sim_model = self._pybullet_client.loadURDF(
|
||||
"humanoid/humanoid.urdf", [0,0.889540259,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
|
||||
|
||||
#self._pybullet_client.setCollisionFilterGroupMask(self._sim_model,-1,collisionFilterGroup=0,collisionFilterMask=0)
|
||||
#for j in range (self._pybullet_client.getNumJoints(self._sim_model)):
|
||||
# self._pybullet_client.setCollisionFilterGroupMask(self._sim_model,j,collisionFilterGroup=0,collisionFilterMask=0)
|
||||
|
||||
|
||||
self._end_effectors = [5,8,11,14] #ankle and wrist, both left and right
|
||||
|
||||
self._kin_model = self._pybullet_client.loadURDF(
|
||||
"humanoid/humanoid.urdf", [0,12.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
|
||||
"humanoid/humanoid.urdf", [0,0.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
|
||||
|
||||
self._pybullet_client.changeDynamics(self._sim_model, -1, lateralFriction=0.9)
|
||||
for j in range (self._pybullet_client.getNumJoints(self._sim_model)):
|
||||
@ -35,6 +42,18 @@ class HumanoidStablePD(object):
|
||||
|
||||
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):
|
||||
@ -126,9 +145,9 @@ class HumanoidStablePD(object):
|
||||
keyFrameDuration = self._mocap_data.KeyFrameDuraction()
|
||||
cycleTime = self.getCycleTime()
|
||||
#print("self._motion_data.NumFrames()=",self._mocap_data.NumFrames())
|
||||
cycles = self.calcCycleCount(t, cycleTime)
|
||||
self._cycleCount = self.calcCycleCount(t, cycleTime)
|
||||
#print("cycles=",cycles)
|
||||
frameTime = t - cycles*cycleTime
|
||||
frameTime = t - self._cycleCount*cycleTime
|
||||
if (frameTime<0):
|
||||
frameTime += cycleTime
|
||||
|
||||
@ -143,12 +162,29 @@ class HumanoidStablePD(object):
|
||||
|
||||
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]
|
||||
|
||||
pose = self._poseInterpolator.Slerp(frameFraction, frameData, frameDataNext, self._pybullet_client)
|
||||
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
|
||||
|
||||
|
||||
@ -186,8 +222,63 @@ class HumanoidStablePD(object):
|
||||
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
|
||||
|
||||
|
||||
#self._jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=4
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,chest,controlMode, targetPosition=self._poseInterpolator._chestRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=4
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,neck,controlMode,targetPosition=self._poseInterpolator._neckRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=4
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightHip,controlMode,targetPosition=self._poseInterpolator._rightHipRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=1
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightKnee,controlMode,targetPosition=self._poseInterpolator._rightKneeRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=4
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightAnkle,controlMode,targetPosition=self._poseInterpolator._rightAnkleRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = [maxForces[startIndex],maxForces[startIndex+1],maxForces[startIndex+2],maxForces[startIndex+3]]
|
||||
startIndex+=4
|
||||
maxForce = maxForces[startIndex]
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=1
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightElbow, controlMode,targetPosition=self._poseInterpolator._rightElbowRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=4
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftHip, controlMode,targetPosition=self._poseInterpolator._leftHipRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=1
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftKnee, controlMode,targetPosition=self._poseInterpolator._leftKneeRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=4
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftAnkle, controlMode,targetPosition=self._poseInterpolator._leftAnkleRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = maxForces[startIndex]
|
||||
startIndex+=4
|
||||
self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftShoulder, controlMode,targetPosition=self._poseInterpolator._leftShoulderRot,positionGain=kp, force=[maxForce])
|
||||
maxForce = 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)
|
||||
@ -317,13 +408,32 @@ class HumanoidStablePD(object):
|
||||
|
||||
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 #0.15
|
||||
root_w = 0#0.2
|
||||
com_w = 0#0.1
|
||||
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
|
||||
@ -386,8 +496,21 @@ class HumanoidStablePD(object):
|
||||
num_joints = 15
|
||||
|
||||
root_rot_w = mJointWeights[root_id]
|
||||
#pose_err += root_rot_w * cKinTree::CalcRootRotErr(joint_mat, pose0, pose1)
|
||||
#vel_err += root_rot_w * cKinTree::CalcRootAngVelErr(joint_mat, vel0, vel1)
|
||||
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
|
||||
@ -418,35 +541,27 @@ class HumanoidStablePD(object):
|
||||
pose_err += w * curr_pose_err
|
||||
vel_err += w * curr_vel_err
|
||||
|
||||
# bool is_end_eff = sim_char.IsEndEffector(j)
|
||||
# if (is_end_eff)
|
||||
# {
|
||||
# tVector pos0 = sim_char.CalcJointPos(j)
|
||||
# tVector pos1 = cKinTree::CalcJointWorldPos(joint_mat, pose1, j)
|
||||
# double ground_h0 = mGround->SampleHeight(pos0)
|
||||
# double ground_h1 = kin_char.GetOriginPos()[1]
|
||||
#
|
||||
# tVector pos_rel0 = pos0 - root_pos0
|
||||
# tVector pos_rel1 = pos1 - root_pos1
|
||||
# pos_rel0[1] = pos0[1] - ground_h0
|
||||
# pos_rel1[1] = pos1[1] - ground_h1
|
||||
#
|
||||
# pos_rel0 = origin_trans * pos_rel0
|
||||
# pos_rel1 = kin_origin_trans * pos_rel1
|
||||
#
|
||||
# curr_end_err = (pos_rel1 - pos_rel0).squaredNorm()
|
||||
# end_eff_err += curr_end_err;
|
||||
# ++num_end_effs;
|
||||
# }
|
||||
#}
|
||||
#if (num_end_effs > 0):
|
||||
# end_eff_err /= num_end_effs
|
||||
#
|
||||
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_err = (root_pos0 - root_pos1).squaredNorm()
|
||||
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
|
||||
@ -454,10 +569,8 @@ class HumanoidStablePD(object):
|
||||
#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
|
||||
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)
|
||||
@ -470,7 +583,13 @@ class HumanoidStablePD(object):
|
||||
|
||||
reward = pose_w * pose_reward + vel_w * vel_reward + end_eff_w * end_eff_reward + root_w * root_reward + com_w * com_reward
|
||||
|
||||
#print("reward = %f (pose_reward=%f, vel_reward=%f, end_eff_reward=%f, root_reward=%f, com_reward=%f)\n", 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
|
||||
|
||||
@ -12,18 +12,18 @@ import random
|
||||
|
||||
|
||||
class PyBulletDeepMimicEnv(Env):
|
||||
def __init__(self, args=None, enable_draw=False, pybullet_client=None):
|
||||
super().__init__(args, enable_draw)
|
||||
def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None):
|
||||
super().__init__(arg_parser, enable_draw)
|
||||
self._num_agents = 1
|
||||
self._pybullet_client = pybullet_client
|
||||
self._isInitialized = False
|
||||
self._useStablePD = True
|
||||
self._arg_parser = arg_parser
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
|
||||
|
||||
startTime = 0. #float(rn)/rnrange * self._humanoid.getCycleTime()
|
||||
self.t = startTime
|
||||
if not self._isInitialized:
|
||||
if self.enable_draw:
|
||||
self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
|
||||
@ -43,16 +43,20 @@ class PyBulletDeepMimicEnv(Env):
|
||||
self._pybullet_client.changeDynamics(self._planeId, linkIndex=-1, lateralFriction=0.9)
|
||||
|
||||
self._mocapData = motion_capture_data.MotionCaptureData()
|
||||
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
|
||||
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
|
||||
motion_file = self._arg_parser.parse_strings('motion_file')
|
||||
print("motion_file=",motion_file[0])
|
||||
|
||||
motionPath = pybullet_data.getDataPath()+"/"+motion_file[0]
|
||||
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
self._mocapData.Load(motionPath)
|
||||
timeStep = 1./600
|
||||
timeStep = 1./600.
|
||||
useFixedBase=False
|
||||
self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData, timeStep, useFixedBase)
|
||||
self._isInitialized = True
|
||||
|
||||
self._pybullet_client.setTimeStep(timeStep)
|
||||
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
|
||||
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1)
|
||||
|
||||
|
||||
selfCheck = False
|
||||
@ -73,6 +77,8 @@ class PyBulletDeepMimicEnv(Env):
|
||||
#startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
|
||||
rnrange = 1000
|
||||
rn = random.randint(0,rnrange)
|
||||
startTime = float(rn)/rnrange * self._humanoid.getCycleTime()
|
||||
self.t = startTime
|
||||
|
||||
self._humanoid.setSimTime(startTime)
|
||||
|
||||
@ -247,21 +253,30 @@ class PyBulletDeepMimicEnv(Env):
|
||||
def log_val(self, agent_id, val):
|
||||
pass
|
||||
|
||||
|
||||
def update(self, timeStep):
|
||||
#print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t)
|
||||
self._pybullet_client.setTimeStep(timeStep)
|
||||
self._humanoid._timeStep = timeStep
|
||||
|
||||
for i in range(1):
|
||||
self.t += timeStep
|
||||
self._humanoid.setSimTime(self.t)
|
||||
|
||||
if self.desiredPose:
|
||||
#kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
#self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=False)
|
||||
kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
|
||||
self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=True)
|
||||
#pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model)
|
||||
#self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0]+3,pos[1],pos[2]],orn)
|
||||
#print("desiredPositions=",self.desiredPose)
|
||||
maxForces = [0,0,0,0,0,0,0,200,200,200,200, 50,50,50,50, 200,200,200,200, 150, 90,90,90,90, 100,100,100,100, 60, 200,200,200,200, 150, 90, 90, 90, 90, 100,100,100,100, 60]
|
||||
|
||||
if self._useStablePD:
|
||||
taus = self._humanoid.computePDForces(self.desiredPose, desiredVelocities=None, maxForces=maxForces)
|
||||
self._humanoid.applyPDForces(taus)
|
||||
else:
|
||||
self._humanoid.setJointMotors(self.desiredPose, maxForces=maxForces)
|
||||
|
||||
self._pybullet_client.stepSimulation()
|
||||
|
||||
|
||||
|
||||
@ -14,17 +14,18 @@ pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
z2y = pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0])
|
||||
#planeId = pybullet_client.loadURDF("plane.urdf",[0,0,0],z2y)
|
||||
planeId= pybullet_client.loadURDF("plane_implicit.urdf",[0,0,0],z2y, useMaximalCoordinates=True)
|
||||
|
||||
pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9)
|
||||
#print("planeId=",planeId)
|
||||
|
||||
pybullet_client.configureDebugVisualizer(pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
|
||||
pybullet_client.setGravity(0,-9.8,0)
|
||||
|
||||
pybullet_client.setPhysicsEngineParameter(numSolverIterations=10)
|
||||
pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9)
|
||||
|
||||
|
||||
mocapData = motion_capture_data.MotionCaptureData()
|
||||
#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
|
||||
motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
#motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
|
||||
motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt"
|
||||
mocapData.Load(motionPath)
|
||||
timeStep = 1./600
|
||||
useFixedBase=False
|
||||
@ -94,23 +95,15 @@ while (1):
|
||||
#humanoid.initializePose(pose=pose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True)
|
||||
#humanoid.resetPose()
|
||||
|
||||
action = [-6.541649997234344482e-02,1.138873845338821411e-01,-1.215099543333053589e-01,4.610761404037475586e-01,-4.278013408184051514e-01,
|
||||
4.064738750457763672e-02,7.801693677902221680e-02,4.934634566307067871e-01,1.321935355663299561e-01,-1.393979601562023163e-02,-6.699572503566741943e-02,
|
||||
4.778462052345275879e-01,3.740053176879882812e-01,-3.230125308036804199e-01,-3.549785539507865906e-02,-3.283375874161720276e-03,5.070925354957580566e-01,
|
||||
1.033667206764221191e+00,-3.644275963306427002e-01,-3.374500572681427002e-02,1.294951438903808594e-01,-5.880850553512573242e-01,
|
||||
1.185980588197708130e-01,6.445263326168060303e-02,1.625719368457794189e-01,4.615224599838256836e-01,3.817881345748901367e-01,-4.382217228412628174e-01,
|
||||
1.626710966229438782e-02,-4.743926972150802612e-02,3.833046853542327881e-01,1.067031383514404297e+00,3.039606213569641113e-01,
|
||||
-1.891726106405258179e-01,3.595829010009765625e-02,-7.283059358596801758e-01]
|
||||
|
||||
pos_tar2 = humanoid.convertActionToPose(action)
|
||||
desiredPose = np.array(pos_tar2)
|
||||
#desiredPose = humanoid.computePose(humanoid._frameFraction)
|
||||
#desiredPose = targetPose.GetPose()
|
||||
desiredPose = humanoid.computePose(humanoid._frameFraction)
|
||||
#desiredPose = desiredPose.GetPose()
|
||||
#curPose = HumanoidPoseInterpolator()
|
||||
#curPose.reset()
|
||||
s = humanoid.getState()
|
||||
#np.savetxt("pb_record_state_s.csv", s, delimiter=",")
|
||||
taus = humanoid.computePDForces(desiredPose)
|
||||
maxForces = [0,0,0,0,0,0,0,200,200,200,200, 50,50,50,50, 200,200,200,200, 150, 90,90,90,90, 100,100,100,100, 60, 200,200,200,200, 150, 90, 90, 90, 90, 100,100,100,100, 60]
|
||||
taus = humanoid.computePDForces(desiredPose, desiredVelocities=None, maxForces=maxForces)
|
||||
|
||||
#print("taus=",taus)
|
||||
humanoid.applyPDForces(taus)
|
||||
|
||||
@ -15,11 +15,11 @@ from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMim
|
||||
import sys
|
||||
import random
|
||||
|
||||
update_timestep = 1./600.
|
||||
update_timestep = 1./240.
|
||||
animating = True
|
||||
|
||||
def update_world(world, time_elapsed):
|
||||
timeStep = 1./600.
|
||||
timeStep = update_timestep
|
||||
world.update(timeStep)
|
||||
reward = world.env.calc_reward(agent_id=0)
|
||||
#print("reward=",reward)
|
||||
@ -48,7 +48,7 @@ args = sys.argv[1:]
|
||||
def build_world(args, enable_draw):
|
||||
arg_parser = build_arg_parser(args)
|
||||
print("enable_draw=",enable_draw)
|
||||
env = PyBulletDeepMimicEnv(args, enable_draw)
|
||||
env = PyBulletDeepMimicEnv(arg_parser, enable_draw)
|
||||
world = RLWorld(env, arg_parser)
|
||||
#world.env.set_playback_speed(playback_speed)
|
||||
|
||||
@ -71,7 +71,7 @@ def build_world(args, enable_draw):
|
||||
print("agent_type=",agent_type)
|
||||
agent = PPOAgent(world, id, json_data)
|
||||
|
||||
agent.set_enable_training(True)
|
||||
agent.set_enable_training(False)
|
||||
world.reset()
|
||||
return world
|
||||
|
||||
@ -83,7 +83,7 @@ if __name__ == '__main__':
|
||||
|
||||
world = build_world(args, True)
|
||||
while (world.env._pybullet_client.isConnected()):
|
||||
timeStep = 1./600.
|
||||
timeStep = update_timestep
|
||||
keys = world.env.getKeyboardEvents()
|
||||
|
||||
|
||||
|
||||
@ -10960,6 +10960,8 @@ initpybullet(void)
|
||||
PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);
|
||||
PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp);
|
||||
PyModule_AddIntConstant(m, "ACTIVATION_STATE_SLEEP", eActivationStateSleep);
|
||||
PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_WAKEUP", eActivationStateEnableWakeup);
|
||||
PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_WAKEUP", eActivationStateDisableWakeup);
|
||||
|
||||
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
|
||||
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);
|
||||
|
||||
2
setup.py
2
setup.py
@ -454,7 +454,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
|
||||
|
||||
setup(
|
||||
name = 'pybullet',
|
||||
version='2.4.3',
|
||||
version='2.4.5',
|
||||
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
|
||||
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
|
||||
url='https://github.com/bulletphysics/bullet3',
|
||||
|
||||
@ -106,6 +106,7 @@ btMultiBody::btMultiBody(int n_links,
|
||||
m_fixedBase(fixedBase),
|
||||
m_awake(true),
|
||||
m_canSleep(canSleep),
|
||||
m_canWakeup(true),
|
||||
m_sleepTimer(0),
|
||||
m_userObjectPointer(0),
|
||||
m_userIndex2(-1),
|
||||
@ -1882,6 +1883,8 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
|
||||
btScalar motion = 0;
|
||||
{
|
||||
@ -1900,9 +1903,12 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
|
||||
else
|
||||
{
|
||||
m_sleepTimer = 0;
|
||||
if (m_canWakeup)
|
||||
{
|
||||
if (!m_awake)
|
||||
wakeUp();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
|
||||
|
||||
@ -453,15 +453,27 @@ public:
|
||||
// sleeping
|
||||
//
|
||||
void setCanSleep(bool canSleep)
|
||||
{
|
||||
if (m_canWakeup)
|
||||
{
|
||||
m_canSleep = canSleep;
|
||||
}
|
||||
}
|
||||
|
||||
bool getCanSleep() const
|
||||
{
|
||||
return m_canSleep;
|
||||
}
|
||||
|
||||
bool getCanWakeup() const
|
||||
{
|
||||
return m_canWakeup;
|
||||
}
|
||||
|
||||
void setCanWakeup(bool canWakeup)
|
||||
{
|
||||
m_canWakeup = canWakeup;
|
||||
}
|
||||
bool isAwake() const { return m_awake; }
|
||||
void wakeUp();
|
||||
void goToSleep();
|
||||
@ -696,6 +708,7 @@ private:
|
||||
// Sleep parameters.
|
||||
bool m_awake;
|
||||
bool m_canSleep;
|
||||
bool m_canWakeup;
|
||||
btScalar m_sleepTimer;
|
||||
|
||||
void *m_userObjectPointer;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user