diff --git a/examples/CommonInterfaces/CommonFileIOInterface.h b/examples/CommonInterfaces/CommonFileIOInterface.h index e41faf78a..beb25edd6 100644 --- a/examples/CommonInterfaces/CommonFileIOInterface.h +++ b/examples/CommonInterfaces/CommonFileIOInterface.h @@ -22,7 +22,7 @@ struct CommonFileIOInterface virtual bool findResourcePath(const char* fileName, char* resourcePathOut, int resourcePathMaxNumBytes)=0; virtual char* readLine(int fileHandle, char* destBuffer, int numBytes)=0; virtual int getFileSize(int fileHandle)=0; - + virtual void enableFileCaching(bool enable) = 0; }; #endif //COMMON_FILE_IO_INTERFACE_H \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 3517027fc..40eff6ae8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -4,6 +4,7 @@ #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Matrix3x3.h" #include "Bullet3Common/b3Transform.h" +#include "Bullet3Common/b3TransformUtil.h" #include #include "SharedMemoryCommands.h" @@ -5264,3 +5265,46 @@ B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double outOrn[2] = result[2]; outOrn[3] = result[3]; } + +B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/]) +{ + b3Quaternion start(startQuat[0], startQuat[1], startQuat[2], startQuat[3]); + b3Quaternion end(endQuat[0], endQuat[1], endQuat[2], endQuat[3]); + b3Vector3 pos=b3MakeVector3(0, 0, 0); + b3Vector3 linVel, angVel; + b3TransformUtil::calculateVelocityQuaternion(pos, pos, start, end, deltaTime, linVel, angVel); + angVelOut[0] = angVel[0]; + angVelOut[1] = angVel[1]; + angVelOut[2] = angVel[2]; +} + +B3_SHARED_API void b3GetQuaternionFromAxisAngle(const double axis[/*3*/], double angle, double outQuat[/*4*/]) +{ + b3Quaternion quat(b3MakeVector3(axis[0], axis[1], axis[2]), angle); + outQuat[0] = quat[0]; + outQuat[1] = quat[1]; + outQuat[2] = quat[2]; + outQuat[3] = quat[3]; +} +B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle) +{ + b3Quaternion q(quat[0], quat[1], quat[2], quat[3]); + b3Vector3 ax = q.getAxis(); + axis[0] = ax[0]; + axis[1] = ax[1]; + axis[2] = ax[2]; + *angle = q.getAngle(); +} + +B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/]) +{ + b3Quaternion orn0(startQuat[0], startQuat[1], startQuat[2], startQuat[3]); + b3Quaternion orn1a(endQuat[0], endQuat[1], endQuat[2], endQuat[3]); + b3Quaternion orn1 = orn0.nearest(orn1a); + b3Quaternion dorn = orn1 * orn0.inverse(); + outOrn[0] = dorn[0]; + outOrn[1] = dorn[1]; + outOrn[2] = dorn[2]; + outOrn[3] = dorn[3]; +} + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 5c68c9748..14ce0f706 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -571,7 +571,7 @@ extern "C" // Sets the number of threads to use to compute the ray intersections for the batch. Specify 0 to let Bullet decide, 1 (default) for single core execution, 2 or more to select the number of threads to use. B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle, int numThreads); //max num rays for b3RaycastBatchAddRay is MAX_RAY_INTERSECTION_BATCH_SIZE - B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]); + B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[/*3*/], const double rayToWorld[/*3*/]); //max num rays for b3RaycastBatchAddRays is MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorld, const double* rayToWorld, int numRays); B3_SHARED_API void b3RaycastBatchSetParentObject(b3SharedMemoryCommandHandle commandHandle, int parentObjectUniqueId, int parentLinkIndex); @@ -635,6 +635,12 @@ extern "C" B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double endQuat[/*4*/], double interpolationFraction, double outOrn[/*4*/]); + B3_SHARED_API void b3GetQuaternionFromAxisAngle(const double axis[/*3*/], double angle, double outQuat[/*4*/]); + B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle); + B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/]); + B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/]); + + #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index bba891478..6c8e8c496 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8004,6 +8004,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_FILE_CACHING) { b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching); + m_data->m_pluginManager.getFileIOInterface()->enableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching!=0); } SharedMemoryStatus& serverCmd = serverStatusOut; diff --git a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp index 9e53b9f77..3d3183ef3 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp +++ b/examples/SharedMemory/plugins/fileIOPlugin/fileIOPlugin.cpp @@ -283,6 +283,11 @@ struct InMemoryFileIO : public CommonFileIOInterface } return 0; } + + virtual void enableFileCaching(bool enable) + { + (void)enable; + } }; struct WrapperFileIO : public CommonFileIOInterface @@ -292,10 +297,12 @@ struct WrapperFileIO : public CommonFileIOInterface WrapperFileHandle m_wrapperFileHandles[B3_MAX_FILEIO_INTERFACES]; InMemoryFileIO m_cachedFiles; + bool m_enableFileCaching; WrapperFileIO() :CommonFileIOInterface(0,0), - m_numWrapperInterfaces(0) + m_numWrapperInterfaces(0), + m_enableFileCaching(true) { for (int i=0;ifileClose(childHandle); break; } @@ -525,6 +535,15 @@ struct WrapperFileIO : public CommonFileIOInterface return numBytes; } + virtual void enableFileCaching(bool enable) + { + m_enableFileCaching = enable; + if (!enable) + { + m_cachedFiles.clearCache(); + } + } + }; diff --git a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h index a0e8fe00f..8c195c8f9 100644 --- a/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h +++ b/examples/SharedMemory/plugins/fileIOPlugin/zipFileIO.h @@ -252,5 +252,10 @@ struct ZipFileIO : public CommonFileIOInterface } return size; } - + + virtual void enableFileCaching(bool enable) + { + (void)enable; + } + }; diff --git a/examples/Utils/b3BulletDefaultFileIO.h b/examples/Utils/b3BulletDefaultFileIO.h index b4cf20c4b..fa972de01 100644 --- a/examples/Utils/b3BulletDefaultFileIO.h +++ b/examples/Utils/b3BulletDefaultFileIO.h @@ -186,6 +186,11 @@ struct b3BulletDefaultFileIO : public CommonFileIOInterface } return size; } + + virtual void enableFileCaching(bool enable) + { + (void) enable; + } }; #endif //B3_BULLET_DEFAULT_FILE_IO_H diff --git a/examples/pybullet/examples/fileIOPlugin.py b/examples/pybullet/examples/fileIOPlugin.py new file mode 100644 index 000000000..99a5bac62 --- /dev/null +++ b/examples/pybullet/examples/fileIOPlugin.py @@ -0,0 +1,20 @@ +import pybullet as p +import time + +p.connect(p.GUI) +fileIO = p.loadPlugin("fileIOPlugin") +if (fileIO>=0): + p.executePluginCommand(fileIO, "pickup.zip", [p.AddFileIOAction, p.ZipFileIO]) + objs= p.loadSDF("pickup/model.sdf") + dobot =objs[0] + p.changeVisualShape(dobot,-1,rgbaColor=[1,1,1,1]) + +else: + print("fileIOPlugin is disabled.") + + +p.setPhysicsEngineParameter(enableFileCaching=False) + +while (1): + p.stepSimulation() + time.sleep(1./240.) \ No newline at end of file diff --git a/examples/pybullet/examples/humanoidMotionCapture.py b/examples/pybullet/examples/humanoidMotionCapture.py index 71025e5ed..c823dc1de 100644 --- a/examples/pybullet/examples/humanoidMotionCapture.py +++ b/examples/pybullet/examples/humanoidMotionCapture.py @@ -16,7 +16,7 @@ p.setPhysicsEngineParameter(numSolverIterations=200) #path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt" -p.loadURDF("plane.urdf",[0,0,-1]) +p.loadURDF("plane.urdf",[0,0,-0.03]) print("path = ", path) with open(path, 'r') as f: motion_dict = json.load(f) diff --git a/examples/pybullet/examples/pickup.zip b/examples/pybullet/examples/pickup.zip new file mode 100644 index 000000000..a3b24573f Binary files /dev/null and b/examples/pybullet/examples/pickup.zip differ diff --git a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf index b84e98b4f..9a3d4938e 100644 --- a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf +++ b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf @@ -1,8 +1,8 @@ - + - + @@ -20,7 +20,7 @@ - + diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py b/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py new file mode 100644 index 000000000..4a5dd01d9 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/mimic/humanoid.py @@ -0,0 +1,289 @@ +import os, inspect +import math +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + +class HumanoidPose(object): + def __init__(self): + pass + + def Reset(self): + + self._basePos = [0,0,0] + self._baseLinVel = [0,0,0] + self._baseOrn = [0,0,0,1] + self._baseAngVel = [0,0,0] + + self._chestRot = [0,0,0,1] + self._chestVel = [0,0,0] + self._neckRot = [0,0,0,1] + self._neckVel = [0,0,0] + + self._rightHipRot = [0,0,0,1] + self._rightHipVel = [0,0,0] + self._rightKneeRot = [0] + self._rightKneeVel = [0] + self._rightAnkleRot = [0,0,0,1] + self._rightAnkleVel = [0,0,0] + + self._rightShoulderRot = [0,0,0,1] + self._rightShoulderVel = [0,0,0] + self._rightElbowRot = [0] + self._rightElbowVel = [0] + + self._leftHipRot = [0,0,0,1] + self._leftHipVel = [0,0,0] + self._leftKneeRot = [0] + self._leftKneeVel = [0] + self._leftAnkleRot = [0,0,0,1] + self._leftAnkleVel = [0,0,0] + + self._leftShoulderRot = [0,0,0,1] + self._leftShoulderVel = [0,0,0] + self._leftElbowRot = [0] + self._leftElbowVel = [0] + + def ComputeLinVel(self,posStart, posEnd, deltaTime): + vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime] + return vel + + def ComputeAngVel(self,ornStart, ornEnd, deltaTime, bullet_client): + dorn = bullet_client.getDifferenceQuaternion(ornStart,ornEnd) + axis,angle = bullet_client.getAxisAngleFromQuaternion(dorn) + angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime] + return angVel + + def NormalizeQuaternion(self, orn): + length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]+orn[3]*orn[3] + if (length2>0): + length = math.sqrt(length2) + print("Normalize? length=",length) + + + def PostProcessMotionData(self, frameData): + baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] + self.NormalizeQuaternion(baseOrn1Start) + chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] + + neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]] + rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]] + rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]] + rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]] + leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] + leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] + leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] + + + def Slerp(self, frameFraction, frameData, frameDataNext,bullet_client ): + keyFrameDuration = frameData[0] + basePos1Start = [frameData[1],frameData[2],frameData[3]] + basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]] + self._basePos = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), + basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), + basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])] + self._baseLinVel = self.ComputeLinVel(basePos1Start,basePos1End, keyFrameDuration) + baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] + baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]] + self._baseOrn = bullet_client.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction) + self._baseAngVel = self.ComputeAngVel(baseOrn1Start,baseOrn1Next, keyFrameDuration, bullet_client) + + ##pre-rotate to make z-up + #y2zPos=[0,0,0.0] + #y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) + #basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) + + chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] + chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]] + self._chestRot = bullet_client.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction) + self._chestVel = self.ComputeAngVel(chestRotStart,chestRotEnd,keyFrameDuration,bullet_client) + + neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]] + neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]] + self._neckRot = bullet_client.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction) + self._neckVel = self.ComputeAngVel(neckRotStart,neckRotEnd,keyFrameDuration,bullet_client) + + rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]] + rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]] + self._rightHipRot = bullet_client.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction) + self._rightHipVel = self.ComputeAngVel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client) + + rightKneeRotStart = [frameData[20]] + rightKneeRotEnd = [frameDataNext[20]] + self._rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])] + self._rightKneeVel = (rightKneeRotEnd[0]-rightKneeRotStart[0])/keyFrameDuration + + rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]] + rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]] + self._rightAnkleRot = bullet_client.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction) + self._rightAnkleVel = self.ComputeAngVel(rightAnkleRotStart,rightAnkleRotEnd,keyFrameDuration,bullet_client) + + rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]] + rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]] + self._rightShoulderRot = bullet_client.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction) + self._rightShoulderVel = self.ComputeAngVel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client) + + rightElbowRotStart = [frameData[29]] + rightElbowRotEnd = [frameDataNext[29]] + self._rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])] + self._rightElbowVel = (rightElbowRotEnd[0]-rightElbowRotStart[0])/keyFrameDuration + + leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] + leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]] + self._leftHipRot = bullet_client.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction) + self._leftHipVel = self.ComputeAngVel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client) + + leftKneeRotStart = [frameData[34]] + leftKneeRotEnd = [frameDataNext[34]] + self._leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ] + self._leftKneeVel = (leftKneeRotEnd[0]-leftKneeRotStart[0])/keyFrameDuration + + leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] + leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]] + self._leftAnkleRot = bullet_client.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction) + self._leftAnkleVel = self.ComputeAngVel(leftAnkleRotStart,leftAnkleRotEnd,keyFrameDuration,bullet_client) + + leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] + leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]] + self._leftShoulderRot = bullet_client.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction) + self._leftShoulderVel = self.ComputeAngVel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client) + + leftElbowRotStart = [frameData[43]] + leftElbowRotEnd = [frameDataNext[43]] + self._leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])] + self._leftElbowVel = (leftElbowRotEnd[0]-leftElbowRotStart[0])/keyFrameDuration + + +class Humanoid(object): + def __init__(self, pybullet_client, motion_data, baseShift): + """Constructs a humanoid and reset it to the initial states. + Args: + pybullet_client: The instance of BulletClient to manage different + simulations. + """ + self._baseShift = baseShift + self._pybullet_client = pybullet_client + self._motion_data = motion_data + self._humanoid = self._pybullet_client.loadURDF( + "humanoid/humanoid.urdf", [0,0.9,0],globalScaling=0.25) + + pose = HumanoidPose() + + for i in range (self._motion_data.NumFrames()-1): + frameData = self._motion_data._motion_data['Frames'][i] + pose.PostProcessMotionData(frameData) + + self._pybullet_client.resetBasePositionAndOrientation(self._humanoid,self._baseShift,[0,0,0,1]) + self._pybullet_client.changeDynamics(self._humanoid, -1, linearDamping=0, angularDamping=0) + for j in range (self._pybullet_client.getNumJoints(self._humanoid)): + self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0) + self._pybullet_client.changeVisualShape(self._humanoid, j , rgbaColor=[1,1,1,1]) + + self._initial_state = self._pybullet_client.saveState() + self._allowed_body_parts=[11,14] + self.Reset() + + def Reset(self): + self._pybullet_client.restoreState(self._initial_state) + self.SetFrameTime(0) + pose = self.InitializePoseFromMotionData() + self.ApplyPose(pose, True) + + + def SetFrameTime(self, t): + self._frameTime = t + self._frame = int(self._frameTime) + self._frameNext = self._frame+1 + if (self._frameNext >= self._motion_data.NumFrames()): + self._frameNext = self._frame + self._frameFraction = self._frameTime - self._frame + + 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 + if (p[1]==self._humanoid): + part=p[3] + if (p[2]==self._humanoid): + part=p[4] + if (part not in self._allowed_body_parts): + terminates=True + + return terminates + + + + + + def InitializePoseFromMotionData(self): + frameData = self._motion_data._motion_data['Frames'][self._frame] + frameDataNext = self._motion_data._motion_data['Frames'][self._frameNext] + pose = HumanoidPose() + pose.Slerp(self._frameFraction, frameData, frameDataNext, self._pybullet_client) + return pose + + + def ApplyAction(self, action): + #turn action into pose + pose = HumanoidPose() + #todo: convert action vector into pose + #convert from angle-axis to quaternion for spherical joints + + initializeBase = False + self.ApplyPose(pose, initializeBase) + + def ApplyPose(self, pose, initializeBase): + if (initializeBase): + self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,0,0,1]) + basePos=[pose._basePos[0]+self._baseShift[0],pose._basePos[1]+self._baseShift[1],pose._basePos[2]+self._baseShift[2]] + self._pybullet_client.resetBasePositionAndOrientation(self._humanoid, + basePos, pose._baseOrn) + self._pybullet_client.resetBaseVelocity(self._humanoid, pose._baseLinVel, pose._baseAngVel) + print("resetBaseVelocity=",pose._baseLinVel) + else: + self._pybullet_client.changeVisualShape(self._humanoid, 2 , rgbaColor=[1,1,1,1]) + maxForce=1000 + kp=0.8 + chest=1 + neck=2 + rightShoulder=3 + rightElbow=4 + leftShoulder=6 + leftElbow = 7 + rightHip = 9 + rightKnee=10 + rightAnkle=11 + leftHip = 12 + leftKnee=13 + leftAnkle=14 + controlMode = self._pybullet_client.POSITION_CONTROL + + if (initializeBase): + self._pybullet_client.resetJointStateMultiDof(self._humanoid,chest,pose._chestRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,neck,pose._neckRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightHip,pose._rightHipRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightKnee,pose._rightKneeRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightAnkle,pose._rightAnkleRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightShoulder,pose._rightShoulderRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,rightElbow, pose._rightElbowRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftHip, pose._leftHipRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftKnee, pose._leftKneeRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftAnkle, pose._leftAnkleRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftShoulder, pose._leftShoulderRot) + self._pybullet_client.resetJointStateMultiDof(self._humanoid,leftElbow, pose._leftElbowRot) + + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,chest,controlMode, targetPosition=pose._chestRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,neck,controlMode,targetPosition=pose._neckRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightHip,controlMode,targetPosition=pose._rightHipRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightKnee,controlMode,targetPosition=pose._rightKneeRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightAnkle,controlMode,targetPosition=pose._rightAnkleRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightShoulder,controlMode,targetPosition=pose._rightShoulderRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,rightElbow, controlMode,targetPosition=pose._rightElbowRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftHip, controlMode,targetPosition=pose._leftHipRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftKnee, controlMode,targetPosition=pose._leftKneeRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftAnkle, controlMode,targetPosition=pose._leftAnkleRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftShoulder, controlMode,targetPosition=pose._leftShoulderRot,positionGain=kp, force=maxForce) + self._pybullet_client.setJointMotorControlMultiDof(self._humanoid,leftElbow, controlMode,targetPosition=pose._leftElbowRot,positionGain=kp, force=maxForce) + \ No newline at end of file diff --git a/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py b/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py new file mode 100644 index 000000000..975b2004b --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/mimic/humanoid_test.py @@ -0,0 +1,76 @@ +import os, inspect +currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) +parentdir = os.path.dirname(os.path.dirname(currentdir)) +os.sys.path.insert(0,parentdir) + +from pybullet_envs.deep_mimic.humanoid import Humanoid +from pybullet_utils.bullet_client import BulletClient +from pybullet_envs.deep_mimic.motion_capture_data import MotionCaptureData +import pybullet_data +import pybullet +import time +import random + +bc = BulletClient(connection_mode=pybullet.GUI) +bc.setAdditionalSearchPath(pybullet_data.getDataPath()) +bc.configureDebugVisualizer(bc.COV_ENABLE_Y_AXIS_UP,1) +bc.setGravity(0,-9.8,0) +motion=MotionCaptureData() + +motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"#humanoid3d_spinkick.txt"#humanoid3d_backflip.txt" +motion.Load(motionPath) +print("numFrames = ", motion.NumFrames()) +frameTimeId= bc.addUserDebugParameter("frameTime",0,motion.NumFrames()-1.1,0) + +y2zOrn = bc.getQuaternionFromEuler([-1.57,0,0]) +bc.loadURDF("plane.urdf",[0,-0.08,0], y2zOrn) + +humanoid = Humanoid(bc, motion,[0,0,0]) + +frameTime = 0 +keyFrameDuration = motion.KeyFrameDuraction() +print("keyFrameDuration=",keyFrameDuration) +#for i in range (50): +# bc.stepSimulation() + +stage = 0 + + + + + +def Reset(humanoid): + global frameTime + humanoid.Reset() + frameTime = 0#random.randint(0,motion.NumFrames()-2) + print("RESET frametime=",frameTime) + humanoid.SetFrameTime(frameTime) + pose = humanoid.InitializePoseFromMotionData() + humanoid.ApplyPose(pose, True) + + +Reset(humanoid) +bc.stepSimulation() + +while (1): + #frameTime = bc.readUserDebugParameter(frameTimeId) + #print("keyFrameDuration=",keyFrameDuration) + dt = (1./240.)/keyFrameDuration + #print("dt=",dt) + frameTime += dt + if (frameTime >= (motion.NumFrames())-1.1): + frameTime = motion.NumFrames()-1.1 + #print("frameTime=", frameTime) + humanoid.SetFrameTime(frameTime) + + pose = humanoid.InitializePoseFromMotionData() + + #humanoid.ApplyPose(pose, False)#False, False) + if (humanoid.Terminates()): + Reset(humanoid) + + bc.stepSimulation() + + + time.sleep(1./240.) + diff --git a/examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py b/examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py new file mode 100644 index 000000000..7a9c86b76 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/mimic/motion_capture_data.py @@ -0,0 +1,19 @@ +import json + +class MotionCaptureData(object): + def __init__(self): + self.Reset() + + def Reset(self): + self._motion_data = [] + + def Load(self, path): + with open(path, 'r') as f: + self._motion_data = json.load(f) + + def NumFrames(self): + return len(self._motion_data['Frames']) + + def KeyFrameDuraction(self): + return self._motion_data['Frames'][0][0] + diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5ac3335cb..d25f896c4 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -8809,6 +8809,55 @@ static PyObject* pybullet_invertTransform(PyObject* self, } +static PyObject* pybullet_calculateVelocityQuaternion(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* quatStartObj; + PyObject* quatEndObj; + double quatStart[4]; + double quatEnd[4]; + double deltaTime; + int physicsClientId = 0; + int hasQuatStart = 0; + int hasQuatEnd = 0; + + static char* kwlist[] = { "quaternionStart", "quaternionEnd", "deltaTime", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOd|i", kwlist, &quatStartObj, &quatEndObj, &deltaTime, &physicsClientId)) + { + return NULL; + } + + if (quatStartObj) + { + hasQuatStart = pybullet_internalSetVector4d(quatStartObj, quatStart); + } + + if (quatEndObj) + { + hasQuatEnd = pybullet_internalSetVector4d(quatEndObj, quatEnd); + } + if (hasQuatStart && hasQuatEnd) + { + double angVelOut[3]; + b3CalculateVelocityQuaternion(quatStart, quatEnd, deltaTime, angVelOut); + { + PyObject* pylist; + int i; + pylist = PyTuple_New(3); + for (i = 0; i < 3; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(angVelOut[i])); + return pylist; + } + } + else + { + PyErr_SetString(SpamError, "Require start and end quaternion, each with 4 components [x,y,z,w]."); + return NULL; + } + Py_INCREF(Py_None); + return Py_None; +} + + static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* quatStartObj; @@ -8858,6 +8907,145 @@ static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyO } +static PyObject* pybullet_getAxisAngleFromQuaternion(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + PyObject* quatObj; + double quat[4]; + int hasQuat = 0; + + static char* kwlist[] = { "quaternion", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "O|i", kwlist, &quatObj, &physicsClientId)) + { + return NULL; + } + + if (quatObj) + { + hasQuat = pybullet_internalSetVector4d(quatObj, quat); + } + + if (hasQuat) + { + double axis[3]; + double angle; + b3GetAxisAngleFromQuaternion(quat, axis, &angle); + { + PyObject* pylist2 = PyTuple_New(2); + { + PyObject* axislist; + int i; + axislist = PyTuple_New(3); + for (i = 0; i < 3; i++) + PyTuple_SetItem(axislist, i, PyFloat_FromDouble(axis[i])); + PyTuple_SetItem(pylist2, 0, axislist); + } + PyTuple_SetItem(pylist2, 1, PyFloat_FromDouble(angle)); + + return pylist2; + } + } + else + { + PyErr_SetString(SpamError, "Require a quaternion with 4 components [x,y,z,w]."); + return NULL; + } + Py_INCREF(Py_None); + return Py_None; +} + + +static PyObject* pybullet_getQuaternionFromAxisAngle(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* axisObj; + double axis[3]; + double angle; + int physicsClientId = 0; + int hasAxis = 0; + + static char* kwlist[] = { "axis", "angle","physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "Od|i", kwlist, &axisObj, &angle,&physicsClientId)) + { + return NULL; + } + + if (axisObj) + { + hasAxis = pybullet_internalSetVectord(axisObj, axis); + } + + if (hasAxis) + { + double quatOut[4]; + b3GetQuaternionFromAxisAngle(axis, angle, quatOut); + { + PyObject* pylist; + int i; + pylist = PyTuple_New(4); + for (i = 0; i < 4; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quatOut[i])); + return pylist; + } + } + else + { + PyErr_SetString(SpamError, "Require axis [x,y,z] and angle."); + return NULL; + } + Py_INCREF(Py_None); + return Py_None; +} + + +static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* quatStartObj; + PyObject* quatEndObj; + double quatStart[4]; + double quatEnd[4]; + int physicsClientId = 0; + int hasQuatStart = 0; + int hasQuatEnd = 0; + + static char* kwlist[] = { "quaternionStart", "quaternionEnd", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|i", kwlist, &quatStartObj, &quatEndObj, &physicsClientId)) + { + return NULL; + } + + if (quatStartObj) + { + hasQuatStart = pybullet_internalSetVector4d(quatStartObj, quatStart); + } + + if (quatEndObj) + { + hasQuatEnd = pybullet_internalSetVector4d(quatEndObj, quatEnd); + } + if (hasQuatStart && hasQuatEnd) + { + double quatOut[4]; + b3GetQuaternionDifference(quatStart, quatEnd, quatOut); + { + PyObject* pylist; + int i; + pylist = PyTuple_New(4); + for (i = 0; i < 4; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quatOut[i])); + return pylist; + } + } + else + { + PyErr_SetString(SpamError, "Require start and end quaternion, each with 4 components [x,y,z,w]."); + return NULL; + } + Py_INCREF(Py_None); + return Py_None; +} + + + /// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo /// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, @@ -10115,6 +10303,18 @@ static PyMethodDef SpamMethods[] = { { "getQuaternionSlerp", (PyCFunction)pybullet_getQuaternionSlerp, METH_VARARGS | METH_KEYWORDS, "Compute the spherical interpolation given a start and end quaternion and an interpolation value in range [0..1]" }, + { "getQuaternionFromAxisAngle", (PyCFunction)pybullet_getQuaternionFromAxisAngle, METH_VARARGS | METH_KEYWORDS, + "Compute the quaternion from axis and angle representation." }, + + { "getAxisAngleFromQuaternion", (PyCFunction)pybullet_getAxisAngleFromQuaternion, METH_VARARGS | METH_KEYWORDS, + "Compute the quaternion from axis and angle representation." }, + + { "getDifferenceQuaternion", (PyCFunction)pybullet_getDifferenceQuaternion, METH_VARARGS | METH_KEYWORDS, + "Compute the quaternion difference from two quaternions." }, + + { "calculateVelocityQuaternion", (PyCFunction)pybullet_calculateVelocityQuaternion, METH_VARARGS | METH_KEYWORDS, + "Compute the angular velocity given start and end quaternion and delta time." }, + {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS, "Given an object id, joint positions, joint velocities and joint " "accelerations, compute the joint forces using Inverse Dynamics"}, diff --git a/setup.py b/setup.py index 9c836e725..f4f6b4fed 100644 --- a/setup.py +++ b/setup.py @@ -585,7 +585,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.3.7', + version='2.3.9', 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', diff --git a/src/LinearMath/btThreads.cpp b/src/LinearMath/btThreads.cpp index 7504b15f7..69a86799f 100644 --- a/src/LinearMath/btThreads.cpp +++ b/src/LinearMath/btThreads.cpp @@ -241,7 +241,7 @@ struct ThreadsafeCounter } }; -static btITaskScheduler* gBtTaskScheduler; +static btITaskScheduler* gBtTaskScheduler=0; static int gThreadsRunningCounter = 0; // useful for detecting if we are trying to do nested parallel-for calls static btSpinMutex gThreadsRunningCounterMutex; static ThreadsafeCounter gThreadCounter;