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/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/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/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;