diff --git a/examples/CommonInterfaces/CommonCallbacks.h b/examples/CommonInterfaces/CommonCallbacks.h
index c11c28024..3047d143e 100644
--- a/examples/CommonInterfaces/CommonCallbacks.h
+++ b/examples/CommonInterfaces/CommonCallbacks.h
@@ -11,6 +11,7 @@ typedef void (*b3RenderCallback)();
enum
{
B3G_ESCAPE = 27,
+ B3G_SPACE = 32,
B3G_F1 = 0xff00,
B3G_F2,
B3G_F3,
@@ -40,7 +41,8 @@ enum
B3G_SHIFT,
B3G_CONTROL,
B3G_ALT,
- B3G_RETURN
+ B3G_RETURN,
+
};
#endif
diff --git a/examples/OpenGLWindow/Win32Window.cpp b/examples/OpenGLWindow/Win32Window.cpp
index 38f1d5e48..a25226e44 100644
--- a/examples/OpenGLWindow/Win32Window.cpp
+++ b/examples/OpenGLWindow/Win32Window.cpp
@@ -67,6 +67,11 @@ int getSpecialKeyFromVirtualKeycode(int virtualKeyCode)
switch (virtualKeyCode)
{
+ case VK_SPACE:
+ {
+ keycode = B3G_SPACE;
+ break;
+ }
case VK_RETURN:
{
keycode = B3G_RETURN;
diff --git a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
index 334b5eda1..ca5205f67 100644
--- a/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
+++ b/examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
@@ -2,8 +2,8 @@
-
-
+
+
diff --git a/examples/pybullet/gym/pybullet_data/plane_transparent.mtl b/examples/pybullet/gym/pybullet_data/plane_transparent.mtl
new file mode 100644
index 000000000..8ce3f6b30
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/plane_transparent.mtl
@@ -0,0 +1,14 @@
+newmtl Material
+ Ns 10.0000
+ Ni 1.5000
+ d 1.0000
+ Tr 0.0000
+ Tf 1.0000 1.0000 1.0000
+ illum 2
+ Ka 0.0000 0.0000 0.0000
+ Kd 0.5880 0.5880 0.5880
+ Ks 0.0000 0.0000 0.0000
+ Ke 0.0000 0.0000 0.0000
+ map_Kd checker_blue.png
+
+
diff --git a/examples/pybullet/gym/pybullet_data/plane_transparent.obj b/examples/pybullet/gym/pybullet_data/plane_transparent.obj
new file mode 100644
index 000000000..02e572bcc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/plane_transparent.obj
@@ -0,0 +1,18 @@
+# Blender v2.66 (sub 1) OBJ File: ''
+# www.blender.org
+mtllib plane_transparent.mtl
+o Plane
+v 15.000000 -15.000000 0.000000
+v 15.000000 15.000000 0.000000
+v -15.000000 15.000000 0.000000
+v -15.000000 -15.000000 0.000000
+
+vt 15.000000 0.000000
+vt 15.000000 15.000000
+vt 0.000000 15.000000
+vt 0.000000 0.000000
+
+usemtl Material
+s off
+f 1/1 2/2 3/3
+f 1/1 3/3 4/4
diff --git a/examples/pybullet/gym/pybullet_data/plane_transparent.urdf b/examples/pybullet/gym/pybullet_data/plane_transparent.urdf
new file mode 100644
index 000000000..99fad2902
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/plane_transparent.urdf
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py
index f2b2400ca..8b1378917 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/__init__.py
@@ -1 +1 @@
-from pybullet_envs.deep_mimic.humanoid_deepmimic_gym_env import HumanoidDeepMimicGymEnv
+
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py
index 0a6581c62..18e5d0191 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py
@@ -1,45 +1,51 @@
-from pybullet_envs.minitaur.envs import bullet_client
+from pybullet_utils import bullet_client
import math
class HumanoidPoseInterpolator(object):
def __init__(self):
pass
- def Reset(self):
+ def Reset(self,basePos=[0,0,0], baseOrn=[0,0,0,1],chestRot=[0,0,0,1], neckRot=[0,0,0,1],rightHipRot= [0,0,0,1], rightKneeRot=[0],rightAnkleRot = [0,0,0,1],
+ rightShoulderRot = [0,0,0,1],rightElbowRot = [0], leftHipRot = [0,0,0,1], leftKneeRot = [0],leftAnkleRot = [0,0,0,1],
+ leftShoulderRot = [0,0,0,1] ,leftElbowRot = [0],
+ baseLinVel = [0,0,0],baseAngVel = [0,0,0], chestVel = [0,0,0],neckVel = [0,0,0],rightHipVel = [0,0,0],rightKneeVel = [0],
+ rightAnkleVel = [0,0,0],rightShoulderVel = [0,0,0],rightElbowVel = [0],leftHipVel = [0,0,0],leftKneeVel = [0],leftAnkleVel = [0,0,0],leftShoulderVel = [0,0,0],leftElbowVel = [0]
+ ):
- self._basePos = [0,0,0]
- self._baseLinVel = [0,0,0]
- self._baseOrn = [0,0,0,1]
- self._baseAngVel = [0,0,0]
+ self._basePos = basePos
+ self._baseLinVel = baseLinVel
+ print("HumanoidPoseInterpolator.Reset: baseLinVel = ", baseLinVel)
+ self._baseOrn =baseOrn
+ self._baseAngVel = baseAngVel
- self._chestRot = [0,0,0,1]
- self._chestVel = [0,0,0]
- self._neckRot = [0,0,0,1]
- self._neckVel = [0,0,0]
+ self._chestRot = chestRot
+ self._chestVel =chestVel
+ self._neckRot = neckRot
+ self._neckVel = neckVel
- 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._rightHipRot = rightHipRot
+ self._rightHipVel = rightHipVel
+ self._rightKneeRot =rightKneeRot
+ self._rightKneeVel = rightKneeVel
+ self._rightAnkleRot = rightAnkleRot
+ self._rightAnkleVel = rightAnkleVel
- self._rightShoulderRot = [0,0,0,1]
- self._rightShoulderVel = [0,0,0]
- self._rightElbowRot = [0]
- self._rightElbowVel = [0]
+ self._rightShoulderRot =rightShoulderRot
+ self._rightShoulderVel = rightShoulderVel
+ self._rightElbowRot = rightElbowRot
+ self._rightElbowVel = rightElbowVel
- 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._leftHipRot = leftHipRot
+ self._leftHipVel = leftHipVel
+ self._leftKneeRot = leftKneeRot
+ self._leftKneeVel = leftKneeVel
+ self._leftAnkleRot =leftAnkleRot
+ self._leftAnkleVel = leftAnkleVel
- self._leftShoulderRot = [0,0,0,1]
- self._leftShoulderVel = [0,0,0]
- self._leftElbowRot = [0]
- self._leftElbowVel = [0]
+ self._leftShoulderRot = leftShoulderRot
+ self._leftShoulderVel = leftShoulderVel
+ self._leftElbowRot =leftElbowRot
+ self._leftElbowVel = leftElbowVel
def ComputeLinVel(self,posStart, posEnd, deltaTime):
vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime]
@@ -50,6 +56,14 @@ class HumanoidPoseInterpolator(object):
axis,angle = bullet_client.getAxisAngleFromQuaternion(dorn)
angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
return angVel
+
+ def ComputeAngVelRel(self,ornStart, ornEnd, deltaTime, bullet_client):
+ ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]]
+ pos_diff, q_diff =bullet_client.multiplyTransforms([0,0,0], ornStartConjugate, [0,0,0], ornEnd)
+ axis,angle = bullet_client.getAxisAngleFromQuaternion(q_diff)
+ angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
+ return angVel
+
def NormalizeVector(self, vec):
length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]
@@ -121,17 +135,17 @@ class HumanoidPoseInterpolator(object):
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)
+ self._chestVel = self.ComputeAngVelRel(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)
+ self._neckVel = self.ComputeAngVelRel(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)
+ self._rightHipVel = self.ComputeAngVelRel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client)
rightKneeRotStart = [frameData[20]]
rightKneeRotEnd = [frameDataNext[20]]
@@ -141,12 +155,12 @@ class HumanoidPoseInterpolator(object):
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)
+ self._rightAnkleVel = self.ComputeAngVelRel(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)
+ self._rightShoulderVel = self.ComputeAngVelRel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client)
rightElbowRotStart = [frameData[29]]
rightElbowRotEnd = [frameDataNext[29]]
@@ -156,7 +170,7 @@ class HumanoidPoseInterpolator(object):
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)
+ self._leftHipVel = self.ComputeAngVelRel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client)
leftKneeRotStart = [frameData[34]]
leftKneeRotEnd = [frameDataNext[34]]
@@ -166,12 +180,12 @@ class HumanoidPoseInterpolator(object):
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)
+ self._leftAnkleVel = self.ComputeAngVelRel(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)
+ self._leftShoulderVel = self.ComputeAngVelRel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client)
leftElbowRotStart = [frameData[43]]
leftElbowRotEnd = [frameDataNext[43]]
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py
index 3239a7e6c..c3e7ca54e 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py
@@ -1,5 +1,5 @@
-from pybullet_envs.deep_mimic.env import pd_controller_stable
+from pybullet_utils import pd_controller_stable
from pybullet_envs.deep_mimic.env import humanoid_pose_interpolator
import math
@@ -24,10 +24,10 @@ class HumanoidStablePD(object):
print("LOADING humanoid!")
self._sim_model = self._pybullet_client.loadURDF(
- "humanoid/humanoid.urdf", [0,0.85,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
+ "humanoid/humanoid.urdf", [0,0.889540259,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
self._kin_model = self._pybullet_client.loadURDF(
- "humanoid/humanoid.urdf", [0,2.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER)
+ "humanoid/humanoid.urdf", [0,12.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)):
@@ -64,7 +64,7 @@ class HumanoidStablePD(object):
for dof in self._jointDofCounts:
self._totalDofs += dof
self.setSimTime(0)
- self._maxForce = 6000
+
self.resetPose()
def resetPose(self):
@@ -146,7 +146,9 @@ class HumanoidStablePD(object):
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)
+ #print("self._poseInterpolator.Slerp(", frameFraction,")=", pose)
return pose
@@ -154,27 +156,34 @@ class HumanoidStablePD(object):
pose = self._poseInterpolator.ConvertFromAction(self._pybullet_client, action)
return pose
- def computePDForces(self, desiredPositions, desiredVelocities = None):
+ def computePDForces(self, desiredPositions, desiredVelocities, maxForces):
if desiredVelocities==None:
desiredVelocities = [0]*self._totalDofs
+
+
taus = self._stablePD.computePD(bodyUniqueId=self._sim_model,
jointIndices = self._jointIndicesAll,
desiredPositions = desiredPositions,
desiredVelocities = desiredVelocities,
kps = self._kpOrg,
kds = self._kdOrg,
- maxForces = [self._maxForce]*self._totalDofs,
+ maxForces = maxForces,
timeStep=self._timeStep)
return taus
def applyPDForces(self, taus):
dofIndex=7
+ scaling = 1
for index in range (len(self._jointIndicesAll)):
jointIndex = self._jointIndicesAll[index]
if self._jointDofCounts[index]==4:
- self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,jointIndex,self._pybullet_client.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]])
+ force=[scaling*taus[dofIndex+0],scaling*taus[dofIndex+1],scaling*taus[dofIndex+2]]
+ #print("force[", jointIndex,"]=",force)
+ self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,jointIndex,self._pybullet_client.TORQUE_CONTROL,force=force)
if self._jointDofCounts[index]==1:
- self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, jointIndex, controlMode=self._pybullet_client.TORQUE_CONTROL, force=[taus[dofIndex]])
+ force=[scaling*taus[dofIndex]]
+ #print("force[", jointIndex,"]=",force)
+ self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, jointIndex, controlMode=self._pybullet_client.TORQUE_CONTROL, force=force)
dofIndex+=self._jointDofCounts[index]
@@ -257,6 +266,13 @@ class HumanoidStablePD(object):
for l in linkPosLocal:
stateVector.append(l)
#re-order the quaternion, DeepMimic uses w,x,y,z
+
+ if (linkOrnLocal[3]<0):
+ linkOrnLocal[0]*=-1
+ linkOrnLocal[1]*=-1
+ linkOrnLocal[2]*=-1
+ linkOrnLocal[3]*=-1
+
stateVector.append(linkOrnLocal[3])
stateVector.append(linkOrnLocal[0])
stateVector.append(linkOrnLocal[1])
@@ -268,9 +284,13 @@ class HumanoidStablePD(object):
ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True)
linkLinVel = ls[6]
linkAngVel = ls[7]
- for l in linkLinVel:
+ linkLinVelLocal , unused = self._pybullet_client.multiplyTransforms([0,0,0], rootTransOrn, linkLinVel,[0,0,0,1])
+ #linkLinVelLocal=[linkLinVelLocal[0]-rootPosRel[0],linkLinVelLocal[1]-rootPosRel[1],linkLinVelLocal[2]-rootPosRel[2]]
+ linkAngVelLocal ,unused = self._pybullet_client.multiplyTransforms([0,0,0], rootTransOrn, linkAngVel,[0,0,0,1])
+
+ for l in linkLinVelLocal:
stateVector.append(l)
- for l in linkAngVel:
+ for l in linkAngVelLocal:
stateVector.append(l)
#print("stateVector len=",len(stateVector))
@@ -284,6 +304,9 @@ class HumanoidStablePD(object):
pts = self._pybullet_client.getContactPoints()
for p in pts:
part = -1
+ #ignore self-collision
+ if (p[1]==p[2]):
+ continue
if (p[1]==self._sim_model):
part=p[3]
if (p[2]==self._sim_model):
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py
deleted file mode 100644
index bb0f5f243..000000000
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py
+++ /dev/null
@@ -1,144 +0,0 @@
-import numpy as np
-
-
-
-class PDControllerStableMultiDof(object):
- def __init__(self, pb):
- self._pb = pb
-
- def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
-
- numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
- curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
- #q1 = [desiredPositions[0],desiredPositions[1],desiredPositions[2],desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
- q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
-
- #qdot1 = [0,0,0, 0,0,0,0]
- baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
-
- qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
- qError = [0,0,0, 0,0,0,0]
-
- qIndex=7
- qdotIndex=7
- zeroAccelerations=[0,0,0, 0,0,0,0]
- for i in range (numJoints):
- js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
-
- jointPos=js[0]
- jointVel = js[1]
- q1+=jointPos
-
- if len(js[0])==1:
- desiredPos=desiredPositions[qIndex]
-
- qdiff=desiredPos - jointPos[0]
- qError.append(qdiff)
- zeroAccelerations.append(0.)
- qdot1+=jointVel
- qIndex+=1
- qdotIndex+=1
- if len(js[0])==4:
- desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
- axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
- jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
- qdot1+=jointVelNew
- qError.append(axis[0])
- qError.append(axis[1])
- qError.append(axis[2])
- qError.append(0)
- desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
- zeroAccelerations+=[0.,0.,0.,0.]
- qIndex+=4
- qdotIndex+=4
-
- q = np.array(q1)
- qdot=np.array(qdot1)
-
- qdotdesired = np.array(desiredVelocities)
- qdoterr = qdotdesired-qdot
-
-
- Kp = np.diagflat(kps)
- Kd = np.diagflat(kds)
-
- p = Kp.dot(qError)
-
- #np.savetxt("pb_qError.csv", qError, delimiter=",")
- #np.savetxt("pb_p.csv", p, delimiter=",")
-
- d = Kd.dot(qdoterr)
-
- #np.savetxt("pb_d.csv", d, delimiter=",")
- #np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
-
-
- M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1, flags=1)
-
-
- M2 = np.array(M1)
- #np.savetxt("M2.csv", M2, delimiter=",")
-
- M = (M2 + Kd * timeStep)
-
- #np.savetxt("pbM_tKd.csv",M, delimiter=",")
-
-
-
- c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
-
-
- c = np.array(c1)
- #np.savetxt("pbC.csv",c, delimiter=",")
- A = M
- #p = [0]*43
- b = p + d -c
- #np.savetxt("pb_acc.csv",b, delimiter=",")
- qddot = np.linalg.solve(A, b)
- tau = p + d - Kd.dot(qddot) * timeStep
- #print("len(tau)=",len(tau))
- maxF = np.array(maxForces)
- forces = np.clip(tau, -maxF , maxF )
- return forces
-
-
-
-class PDControllerStable(object):
- def __init__(self, pb):
- self._pb = pb
-
- def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
- numJoints = self._pb.getNumJoints(bodyUniqueId)
- jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
- q1 = []
- qdot1 = []
- zeroAccelerations = []
- for i in range (numJoints):
- q1.append(jointStates[i][0])
- qdot1.append(jointStates[i][1])
- zeroAccelerations.append(0)
- q = np.array(q1)
- qdot=np.array(qdot1)
- qdes = np.array(desiredPositions)
- qdotdes = np.array(desiredVelocities)
- qError = qdes - q
- qdotError = qdotdes - qdot
- Kp = np.diagflat(kps)
- Kd = np.diagflat(kds)
- p = Kp.dot(qError)
- d = Kd.dot(qdotError)
- forces = p + d
-
- M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
- M2 = np.array(M1)
- M = (M2 + Kd * timeStep)
- c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
- c = np.array(c1)
- A = M
- b = -c + p + d
- qddot = np.linalg.solve(A, b)
- tau = p + d - Kd.dot(qddot) * timeStep
- maxF = np.array(maxForces)
- forces = np.clip(tau, -maxF , maxF )
- #print("c=",c)
- return tau
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py
index 272c9df13..3159fcb29 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py
@@ -2,9 +2,9 @@ import numpy as np
import math
from pybullet_envs.deep_mimic.env.env import Env
from pybullet_envs.deep_mimic.env.action_space import ActionSpace
-from pybullet_envs.minitaur.envs import bullet_client
+from pybullet_utils import bullet_client
import time
-import motion_capture_data
+from pybullet_envs.deep_mimic.env import motion_capture_data
from pybullet_envs.deep_mimic.env import humanoid_stable_pd
import pybullet_data
import pybullet as p1
@@ -20,7 +20,10 @@ class PyBulletDeepMimicEnv(Env):
self.reset()
def reset(self):
- self.t = 0
+
+
+ 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)
@@ -29,7 +32,7 @@ class PyBulletDeepMimicEnv(Env):
self._pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath())
z2y = self._pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0])
- self._planeId = self._pybullet_client.loadURDF("plane.urdf",[0,0,0],z2y)
+ self._planeId = self._pybullet_client.loadURDF("plane_implicit.urdf",[0,0,0],z2y, useMaximalCoordinates=True)
#print("planeId=",self._planeId)
self._pybullet_client.configureDebugVisualizer(self._pybullet_client.COV_ENABLE_Y_AXIS_UP,1)
self._pybullet_client.setGravity(0,-9.8,0)
@@ -38,8 +41,8 @@ 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"
+ #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"
+ motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
self._mocapData.Load(motionPath)
timeStep = 1./600
useFixedBase=False
@@ -48,7 +51,7 @@ class PyBulletDeepMimicEnv(Env):
self._pybullet_client.setTimeStep(timeStep)
self._pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
-
+
selfCheck = False
if (selfCheck):
@@ -65,16 +68,17 @@ class PyBulletDeepMimicEnv(Env):
#self._pybullet_client.stepSimulation()
time.sleep(timeStep)
#print("numframes = ", self._humanoid._mocap_data.NumFrames())
- startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
+ #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._humanoid.setSimTime(startTime)
+
self._humanoid.resetPose()
#this clears the contact points. Todo: add API to explicitly clear all contact points?
- self._pybullet_client.stepSimulation()
-
+ #self._pybullet_client.stepSimulation()
+ self._humanoid.resetPose()
+ self.needs_update_time = self.t-1#force update
def get_num_agents(self):
return self._num_agents
@@ -198,12 +202,14 @@ class PyBulletDeepMimicEnv(Env):
self._mode = mode
def need_new_action(self, agent_id):
- return True
+ if self.t>=self.needs_update_time:
+ self.needs_update_time = self.t + 1./30.
+ return True
+ return False
def record_state(self, agent_id):
state = self._humanoid.getState()
- state[1]=state[1]+0.008
- #print("record_state=",state)
+
return np.array(state)
@@ -216,24 +222,43 @@ class PyBulletDeepMimicEnv(Env):
return reward
def set_action(self, agent_id, action):
+ #print("action=",)
+ #for a in action:
+ # print(a)
+ np.savetxt("pb_action.csv", action, delimiter=",")
self.desiredPose = self._humanoid.convertActionToPose(action)
+ #we need the target root positon and orientation to be zero, to be compatible with deep mimic
+ self.desiredPose[0] = 0
+ self.desiredPose[1] = 0
+ self.desiredPose[2] = 0
+ self.desiredPose[3] = 0
+ self.desiredPose[4] = 0
+ self.desiredPose[5] = 0
+ self.desiredPose[6] = 0
+ target_pose = np.array(self.desiredPose)
+
+
+ np.savetxt("pb_target_pose.csv", target_pose, delimiter=",")
+
#print("set_action: desiredPose=", self.desiredPose)
def log_val(self, agent_id, val):
pass
def update(self, timeStep):
- for i in range(10):
+ #print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t)
+ 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)
- pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model)
- self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0],pos[1]+2,pos[2]],orn)
+ #kinPose = self._humanoid.computePose(self._humanoid._frameFraction)
+ #self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=False)
+ #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)
- taus = self._humanoid.computePDForces(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]
+ taus = self._humanoid.computePDForces(self.desiredPose, desiredVelocities=None, maxForces=maxForces)
self._humanoid.applyPDForces(taus)
self._pybullet_client.stepSimulation()
@@ -255,3 +280,13 @@ class PyBulletDeepMimicEnv(Env):
def check_valid_episode(self):
#could check if limbs exceed velocity threshold
return true
+
+ def getKeyboardEvents(self):
+ return self._pybullet_client.getKeyboardEvents()
+
+ def isKeyTriggered(self, keys, key):
+ o = ord(key)
+ #print("ord=",o)
+ if o in keys:
+ return keys[ord(key)] & self._pybullet_client.KEY_WAS_TRIGGERED
+ return False
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py
new file mode 100644
index 000000000..5cf9a5bcc
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py
@@ -0,0 +1,122 @@
+from pybullet_utils import bullet_client
+import time
+import math
+import motion_capture_data
+from pybullet_envs.deep_mimic.env import humanoid_stable_pd
+import pybullet_data
+import pybullet as p1
+import humanoid_pose_interpolator
+import numpy as np
+
+pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI)
+
+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)
+
+#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"
+mocapData.Load(motionPath)
+timeStep = 1./600
+useFixedBase=False
+humanoid = humanoid_stable_pd.HumanoidStablePD(pybullet_client, mocapData, timeStep, useFixedBase)
+isInitialized = True
+
+pybullet_client.setTimeStep(timeStep)
+pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
+timeId = pybullet_client.addUserDebugParameter("time",0,10,0)
+
+
+#startPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
+#startPose.Reset(basePos=[-0.000000,0.889540,0.000000],baseOrn=[0.029215,-0.000525,-0.017963,0.999412],chestRot=[0.000432,0.000572,0.005500,0.999985],
+# neckRot=[0.001660,-0.011168,-0.140597,0.990003],rightHipRot=[-0.024450,-0.000839,-0.038869,0.998945],rightKneeRot=[-0.014186],rightAnkleRot=[0.010757,-0.105223,0.035405,0.993760],
+# rightShoulderRot=[-0.003003,-0.124234,0.073280,0.989539],rightElbowRot=[0.240463],leftHipRot=[-0.020920,-0.012925,-0.006300,0.999678],leftKneeRot=[-0.027859],
+# leftAnkleRot=[-0.010764,0.105284,-0.009276,0.994341],leftShoulderRot=[0.055661,-0.019608,0.098917,0.993344],leftElbowRot=[0.148934],
+# baseLinVel=[-0.340837,0.377742,0.009662],baseAngVel=[0.047057,0.285253,-0.248554],chestVel=[-0.016455,-0.070035,-0.231662],neckVel=[0.072168,0.097898,-0.059063],
+# rightHipVel=[-0.315908,-0.131685,1.114815],rightKneeVel=[0.103419],rightAnkleVel=[-0.409780,-0.099954,-4.241572],rightShoulderVel=[-3.324227,-2.510209,1.834637],
+# rightElbowVel=[-0.212299],leftHipVel=[0.173056,-0.191063,1.226781,0.000000],leftKneeVel=[-0.665322],leftAnkleVel=[0.282716,0.086217,-3.007098,0.000000],
+# leftShoulderVel=[4.253144,2.038637,1.170750],leftElbowVel=[0.387993])
+#
+#targetPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
+#targetPose.Reset(basePos=[0.000000,0.000000,0.000000],baseOrn=[0.000000,0.000000,0.000000,1.000000],chestRot=[-0.006711,0.007196,-0.027119,0.999584],neckRot=[-0.017613,-0.033879,-0.209250,0.977116],
+# rightHipRot=[-0.001697,-0.006510,0.046117,0.998913],rightKneeRot=[0.366954],rightAnkleRot=[0.012605,0.001208,-0.187007,0.982277],rightShoulderRot=[-0.468057,-0.044589,0.161134,0.867739],
+# rightElbowRot=[-0.593650],leftHipRot=[0.006993,0.017242,0.049703,0.998591],leftKneeRot=[0.395147],leftAnkleRot=[-0.008922,0.026517,-0.217852,0.975581],
+# leftShoulderRot=[0.426160,-0.266177,0.044672,0.863447],leftElbowRot=[-0.726281])
+
+#out_tau= [0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-33.338211,-1.381748,-118.708975,0.000000,-2.813919,-2.773850,-0.772481,0.000000,31.885372,2.658243,64.988216,0.000000,94.773133,1.784944,6.240010,5.407563,0.000000,-180.441290,-6.821173,-19.502417,0.000000,-44.518261,9.992627,-2.380950,53.057697,0.000000,111.728594,-1.218496,-4.630812,4.268995,0.000000,89.741829,-8.460265,-117.727884,0.000000,-79.481906]
+#,mSimWorld->stepSimulation(timestep:0.001667, mParams.mNumSubsteps:2, subtimestep:0.000833)
+#cImpPDController::CalcControlForces timestep=0.001667
+
+
+def isKeyTriggered(keys, key):
+ o = ord(key)
+ if o in keys:
+ return keys[ord(key)] & pybullet_client.KEY_WAS_TRIGGERED
+ return False
+
+animating = False
+singleStep = False
+
+#humanoid.initializePose(pose=startPose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True)
+t=0
+while (1):
+
+ keys = pybullet_client.getKeyboardEvents()
+ #print(keys)
+ if isKeyTriggered(keys, ' '):
+ animating = not animating
+
+ if isKeyTriggered(keys, 'b'):
+ singleStep = True
+
+ if animating or singleStep:
+
+
+ singleStep = False
+ #t = pybullet_client.readUserDebugParameter(timeId)
+ #print("t=",t)
+ for i in range (1):
+
+ print("t=",t)
+ humanoid.setSimTime(t)
+
+ humanoid.computePose(humanoid._frameFraction)
+ pose = humanoid._poseInterpolator
+ #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()
+ #curPose = HumanoidPoseInterpolator()
+ #curPose.reset()
+ s = humanoid.getState()
+ #np.savetxt("pb_record_state_s.csv", s, delimiter=",")
+ taus = humanoid.computePDForces(desiredPose)
+
+ #print("taus=",taus)
+ humanoid.applyPDForces(taus)
+
+ pybullet_client.stepSimulation()
+ t+=1./600.
+
+
+ time.sleep(1./600.)
\ No newline at end of file
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py
index 1b9489489..ece705302 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/ppo_agent.py
@@ -147,6 +147,7 @@ class PPOAgent(PGAgent):
def _decide_action(self, s, g):
with self.sess.as_default(), self.graph.as_default():
self._exp_action = self._enable_stoch_policy() and MathUtil.flip_coin(self.exp_params_curr.rate)
+ #print("_decide_action._exp_action=",self._exp_action)
a, logp = self._eval_actor(s, g, self._exp_action)
return a[0], logp[0]
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py
index 25886c659..3e66eb270 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/learning/rl_agent.py
@@ -123,8 +123,7 @@ class RLAgent(ABC):
if self.need_new_action():
#print("update_new_action!!!")
self._update_new_action()
- else:
- print("no action???")
+
if (self._mode == self.Mode.TRAIN and self.enable_training):
self._update_counter += timestep
@@ -329,7 +328,9 @@ class RLAgent(ABC):
return
def _update_new_action(self):
+ #print("_update_new_action!")
s = self._record_state()
+ #np.savetxt("pb_record_state_s.csv", s, delimiter=",")
g = self._record_goal()
if not (self._is_first_step()):
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py
index 0ed1afde8..31b976563 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py
@@ -16,6 +16,7 @@ import sys
import random
update_timestep = 1./600.
+animating = True
def update_world(world, time_elapsed):
timeStep = 1./600.
@@ -74,9 +75,21 @@ def build_world(args, enable_draw):
world.reset()
return world
+
+
+
+
if __name__ == '__main__':
+
world = build_world(args, True)
while (world.env._pybullet_client.isConnected()):
timeStep = 1./600.
- update_world(world, timeStep)
+ keys = world.env.getKeyboardEvents()
+
+
+ if world.env.isKeyTriggered(keys, ' '):
+ animating = not animating
+ if (animating):
+ update_world(world, timeStep)
+ #animating=False
diff --git a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py
new file mode 100644
index 000000000..d0cee30bd
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py
@@ -0,0 +1,193 @@
+import numpy as np
+
+
+
+class PDControllerStableMultiDof(object):
+ def __init__(self, pb):
+ self._pb = pb
+
+ 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 computeAngVelRel(self,ornStart, ornEnd, deltaTime, bullet_client):
+ # ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]]
+ # pos_diff, q_diff =bullet_client.multiplyTransforms([0,0,0], ornStartConjugate, [0,0,0], ornEnd)
+ # axis,angle = bullet_client.getAxisAngleFromQuaternion(q_diff)
+ # angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
+ # return angVel
+
+ def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
+
+ numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId)
+ curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId)
+ q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]]
+ #print("q1=",q1)
+
+
+ #qdot1 = [0,0,0, 0,0,0,0]
+ baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId)
+ #print("baseLinVel=",baseLinVel)
+ qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0]
+ #qError = [0,0,0, 0,0,0,0]
+ desiredOrn = [desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]]
+ axis1 = self._pb.getAxisDifferenceQuaternion(desiredOrn,curOrn)
+ angDiff = [0,0,0]#self.computeAngVel(curOrn, desiredOrn, 1, self._pb)
+ qError=[ desiredPositions[0]-curPos[0], desiredPositions[1]-curPos[1], desiredPositions[2]-curPos[2],angDiff[0],angDiff[1],angDiff[2],0]
+ target_pos = np.array(desiredPositions)
+ #np.savetxt("pb_target_pos.csv", target_pos, delimiter=",")
+
+
+ qIndex=7
+ qdotIndex=7
+ zeroAccelerations=[0,0,0, 0,0,0,0]
+ for i in range (numJoints):
+ js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
+
+ jointPos=js[0]
+ jointVel = js[1]
+ q1+=jointPos
+
+ if len(js[0])==1:
+ desiredPos=desiredPositions[qIndex]
+
+ qdiff=desiredPos - jointPos[0]
+ qError.append(qdiff)
+ zeroAccelerations.append(0.)
+ qdot1+=jointVel
+ qIndex+=1
+ qdotIndex+=1
+ if len(js[0])==4:
+ desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]]
+ #axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos)
+ #angDiff = self.computeAngVelRel(jointPos, desiredPos, 1, self._pb)
+ angDiff = self._pb.computeAngVelRel(jointPos, desiredPos, 1)
+
+ jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0]
+ qdot1+=jointVelNew
+ qError.append(angDiff[0])
+ qError.append(angDiff[1])
+ qError.append(angDiff[2])
+ qError.append(0)
+ desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]]
+ zeroAccelerations+=[0.,0.,0.,0.]
+ qIndex+=4
+ qdotIndex+=4
+
+ q = np.array(q1)
+
+ qerr = np.array(qError)
+
+ #np.savetxt("pb_qerro.csv",qerr,delimiter=",")
+
+ #np.savetxt("pb_q.csv", q, delimiter=",")
+
+ qdot=np.array(qdot1)
+ #np.savetxt("qdot.csv", qdot, delimiter=",")
+
+ qdotdesired = np.array(desiredVelocities)
+ qdoterr = qdotdesired-qdot
+
+
+ Kp = np.diagflat(kps)
+ Kd = np.diagflat(kds)
+
+ p = Kp.dot(qError)
+
+ #np.savetxt("pb_qError.csv", qError, delimiter=",")
+ #np.savetxt("pb_p.csv", p, delimiter=",")
+
+ d = Kd.dot(qdoterr)
+
+ #np.savetxt("pb_d.csv", d, delimiter=",")
+ #np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",")
+
+
+ M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1, flags=1)
+
+
+ M2 = np.array(M1)
+ #np.savetxt("M2.csv", M2, delimiter=",")
+
+ M = (M2 + Kd * timeStep)
+
+ #np.savetxt("pbM_tKd.csv",M, delimiter=",")
+
+
+
+ c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1)
+
+
+ c = np.array(c1)
+ #np.savetxt("pb_C.csv",c, delimiter=",")
+ A = M
+ #p = [0]*43
+ #np.savetxt("pb_kp_dot_qError.csv", p)
+ #np.savetxt("pb_kd_dot_vError.csv", d)
+
+ b = p + d -c
+ #np.savetxt("pb_b_acc.csv",b, delimiter=",")
+
+
+ useNumpySolver = False
+ if useNumpySolver:
+ qddot = np.linalg.solve(A, b)
+ else:
+ dofCount = len(b)
+ print(dofCount)
+ qddot = self._pb.ldltSolve(bodyUniqueId, jointPositions=q1, b=b.tolist(), kd=kds, t=timeStep)
+
+ tau = p + d - Kd.dot(qddot) * timeStep
+ #print("len(tau)=",len(tau))
+ #np.savetxt("pb_tau_not_clamped.csv", tau, delimiter=",")
+
+ maxF = np.array(maxForces)
+ #print("maxF",maxF)
+ forces = np.clip(tau, -maxF , maxF )
+
+ #np.savetxt("pb_tau_clamped.csv", tau, delimiter=",")
+ return forces
+
+
+
+class PDControllerStable(object):
+ def __init__(self, pb):
+ self._pb = pb
+
+ def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
+ numJoints = self._pb.getNumJoints(bodyUniqueId)
+ jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+ q1 = []
+ qdot1 = []
+ zeroAccelerations = []
+ for i in range (numJoints):
+ q1.append(jointStates[i][0])
+ qdot1.append(jointStates[i][1])
+ zeroAccelerations.append(0)
+ q = np.array(q1)
+ qdot=np.array(qdot1)
+ qdes = np.array(desiredPositions)
+ qdotdes = np.array(desiredVelocities)
+ qError = qdes - q
+ qdotError = qdotdes - qdot
+ Kp = np.diagflat(kps)
+ Kd = np.diagflat(kds)
+ p = Kp.dot(qError)
+ d = Kd.dot(qdotError)
+ forces = p + d
+
+ M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
+ M2 = np.array(M1)
+ M = (M2 + Kd * timeStep)
+ c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
+ c = np.array(c1)
+ A = M
+ b = -c + p + d
+ qddot = np.linalg.solve(A, b)
+ tau = p + d - Kd.dot(qddot) * timeStep
+ maxF = np.array(maxForces)
+ forces = np.clip(tau, -maxF , maxF )
+ #print("c=",c)
+ return tau
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index e9403626c..06561ab1f 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -9286,7 +9286,7 @@ static PyObject* pybullet_getDifferenceQuaternion(PyObject* self, PyObject* args
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))
{
@@ -10711,11 +10711,11 @@ static PyMethodDef SpamMethods[] = {
{NULL, NULL, 0, NULL} /* Sentinel */
};
-///copied from CommonWindowInterface.h
-
+///copied from CommonCallbacks.h
enum
{
B3G_ESCAPE = 27,
+ B3G_SPACE = 32,
B3G_F1 = 0xff00,
B3G_F2,
B3G_F3,
@@ -10745,7 +10745,7 @@ enum
B3G_SHIFT,
B3G_CONTROL,
B3G_ALT,
- B3G_RETURN
+ B3G_RETURN,
};
#if PY_MAJOR_VERSION >= 3
@@ -10959,6 +10959,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "B3G_CONTROL", B3G_CONTROL);
PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT);
PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN);
+ PyModule_AddIntConstant(m, "B3G_SPACE", B3G_SPACE);
PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);