1import os, inspect 2import math 3currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) 4parentdir = os.path.dirname(os.path.dirname(currentdir)) 5os.sys.path.insert(0, parentdir) 6 7from pybullet_utils.bullet_client import BulletClient 8import pybullet_data 9 10jointTypes = [ 11 "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED" 12] 13 14 15class HumanoidPose(object): 16 17 def __init__(self): 18 pass 19 20 def Reset(self): 21 22 self._basePos = [0, 0, 0] 23 self._baseLinVel = [0, 0, 0] 24 self._baseOrn = [0, 0, 0, 1] 25 self._baseAngVel = [0, 0, 0] 26 27 self._chestRot = [0, 0, 0, 1] 28 self._chestVel = [0, 0, 0] 29 self._neckRot = [0, 0, 0, 1] 30 self._neckVel = [0, 0, 0] 31 32 self._rightHipRot = [0, 0, 0, 1] 33 self._rightHipVel = [0, 0, 0] 34 self._rightKneeRot = [0] 35 self._rightKneeVel = [0] 36 self._rightAnkleRot = [0, 0, 0, 1] 37 self._rightAnkleVel = [0, 0, 0] 38 39 self._rightShoulderRot = [0, 0, 0, 1] 40 self._rightShoulderVel = [0, 0, 0] 41 self._rightElbowRot = [0] 42 self._rightElbowVel = [0] 43 44 self._leftHipRot = [0, 0, 0, 1] 45 self._leftHipVel = [0, 0, 0] 46 self._leftKneeRot = [0] 47 self._leftKneeVel = [0] 48 self._leftAnkleRot = [0, 0, 0, 1] 49 self._leftAnkleVel = [0, 0, 0] 50 51 self._leftShoulderRot = [0, 0, 0, 1] 52 self._leftShoulderVel = [0, 0, 0] 53 self._leftElbowRot = [0] 54 self._leftElbowVel = [0] 55 56 def ComputeLinVel(self, posStart, posEnd, deltaTime): 57 vel = [(posEnd[0] - posStart[0]) / deltaTime, (posEnd[1] - posStart[1]) / deltaTime, 58 (posEnd[2] - posStart[2]) / deltaTime] 59 return vel 60 61 def ComputeAngVel(self, ornStart, ornEnd, deltaTime, bullet_client): 62 dorn = bullet_client.getDifferenceQuaternion(ornStart, ornEnd) 63 axis, angle = bullet_client.getAxisAngleFromQuaternion(dorn) 64 angVel = [(axis[0] * angle) / deltaTime, (axis[1] * angle) / deltaTime, 65 (axis[2] * angle) / deltaTime] 66 return angVel 67 68 def NormalizeQuaternion(self, orn): 69 length2 = orn[0] * orn[0] + orn[1] * orn[1] + orn[2] * orn[2] + orn[3] * orn[3] 70 if (length2 > 0): 71 length = math.sqrt(length2) 72 #print("Normalize? length=",length) 73 74 def PostProcessMotionData(self, frameData): 75 baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]] 76 self.NormalizeQuaternion(baseOrn1Start) 77 chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]] 78 79 neckRotStart = [frameData[13], frameData[14], frameData[15], frameData[12]] 80 rightHipRotStart = [frameData[17], frameData[18], frameData[19], frameData[16]] 81 rightAnkleRotStart = [frameData[22], frameData[23], frameData[24], frameData[21]] 82 rightShoulderRotStart = [frameData[26], frameData[27], frameData[28], frameData[25]] 83 leftHipRotStart = [frameData[31], frameData[32], frameData[33], frameData[30]] 84 leftAnkleRotStart = [frameData[36], frameData[37], frameData[38], frameData[35]] 85 leftShoulderRotStart = [frameData[40], frameData[41], frameData[42], frameData[39]] 86 87 def Slerp(self, frameFraction, frameData, frameDataNext, bullet_client): 88 keyFrameDuration = frameData[0] 89 basePos1Start = [frameData[1], frameData[2], frameData[3]] 90 basePos1End = [frameDataNext[1], frameDataNext[2], frameDataNext[3]] 91 self._basePos = [ 92 basePos1Start[0] + frameFraction * (basePos1End[0] - basePos1Start[0]), 93 basePos1Start[1] + frameFraction * (basePos1End[1] - basePos1Start[1]), 94 basePos1Start[2] + frameFraction * (basePos1End[2] - basePos1Start[2]) 95 ] 96 self._baseLinVel = self.ComputeLinVel(basePos1Start, basePos1End, keyFrameDuration) 97 baseOrn1Start = [frameData[5], frameData[6], frameData[7], frameData[4]] 98 baseOrn1Next = [frameDataNext[5], frameDataNext[6], frameDataNext[7], frameDataNext[4]] 99 self._baseOrn = bullet_client.getQuaternionSlerp(baseOrn1Start, baseOrn1Next, frameFraction) 100 self._baseAngVel = self.ComputeAngVel(baseOrn1Start, baseOrn1Next, keyFrameDuration, 101 bullet_client) 102 103 ##pre-rotate to make z-up 104 #y2zPos=[0,0,0.0] 105 #y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) 106 #basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) 107 108 chestRotStart = [frameData[9], frameData[10], frameData[11], frameData[8]] 109 chestRotEnd = [frameDataNext[9], frameDataNext[10], frameDataNext[11], frameDataNext[8]] 110 self._chestRot = bullet_client.getQuaternionSlerp(chestRotStart, chestRotEnd, frameFraction) 111 self._chestVel = self.ComputeAngVel(chestRotStart, chestRotEnd, keyFrameDuration, 112 bullet_client) 113 114 neckRotStart = [frameData[13], frameData[14], frameData[15], frameData[12]] 115 neckRotEnd = [frameDataNext[13], frameDataNext[14], frameDataNext[15], frameDataNext[12]] 116 self._neckRot = bullet_client.getQuaternionSlerp(neckRotStart, neckRotEnd, frameFraction) 117 self._neckVel = self.ComputeAngVel(neckRotStart, neckRotEnd, keyFrameDuration, bullet_client) 118 119 rightHipRotStart = [frameData[17], frameData[18], frameData[19], frameData[16]] 120 rightHipRotEnd = [frameDataNext[17], frameDataNext[18], frameDataNext[19], frameDataNext[16]] 121 self._rightHipRot = bullet_client.getQuaternionSlerp(rightHipRotStart, rightHipRotEnd, 122 frameFraction) 123 self._rightHipVel = self.ComputeAngVel(rightHipRotStart, rightHipRotEnd, keyFrameDuration, 124 bullet_client) 125 126 rightKneeRotStart = [frameData[20]] 127 rightKneeRotEnd = [frameDataNext[20]] 128 self._rightKneeRot = [ 129 rightKneeRotStart[0] + frameFraction * (rightKneeRotEnd[0] - rightKneeRotStart[0]) 130 ] 131 self._rightKneeVel = [(rightKneeRotEnd[0] - rightKneeRotStart[0]) / keyFrameDuration] 132 133 rightAnkleRotStart = [frameData[22], frameData[23], frameData[24], frameData[21]] 134 rightAnkleRotEnd = [frameDataNext[22], frameDataNext[23], frameDataNext[24], frameDataNext[21]] 135 self._rightAnkleRot = bullet_client.getQuaternionSlerp(rightAnkleRotStart, rightAnkleRotEnd, 136 frameFraction) 137 self._rightAnkleVel = self.ComputeAngVel(rightAnkleRotStart, rightAnkleRotEnd, 138 keyFrameDuration, bullet_client) 139 140 rightShoulderRotStart = [frameData[26], frameData[27], frameData[28], frameData[25]] 141 rightShoulderRotEnd = [ 142 frameDataNext[26], frameDataNext[27], frameDataNext[28], frameDataNext[25] 143 ] 144 self._rightShoulderRot = bullet_client.getQuaternionSlerp(rightShoulderRotStart, 145 rightShoulderRotEnd, frameFraction) 146 self._rightShoulderVel = self.ComputeAngVel(rightShoulderRotStart, rightShoulderRotEnd, 147 keyFrameDuration, bullet_client) 148 149 rightElbowRotStart = [frameData[29]] 150 rightElbowRotEnd = [frameDataNext[29]] 151 self._rightElbowRot = [ 152 rightElbowRotStart[0] + frameFraction * (rightElbowRotEnd[0] - rightElbowRotStart[0]) 153 ] 154 self._rightElbowVel = [(rightElbowRotEnd[0] - rightElbowRotStart[0]) / keyFrameDuration] 155 156 leftHipRotStart = [frameData[31], frameData[32], frameData[33], frameData[30]] 157 leftHipRotEnd = [frameDataNext[31], frameDataNext[32], frameDataNext[33], frameDataNext[30]] 158 self._leftHipRot = bullet_client.getQuaternionSlerp(leftHipRotStart, leftHipRotEnd, 159 frameFraction) 160 self._leftHipVel = self.ComputeAngVel(leftHipRotStart, leftHipRotEnd, keyFrameDuration, 161 bullet_client) 162 163 leftKneeRotStart = [frameData[34]] 164 leftKneeRotEnd = [frameDataNext[34]] 165 self._leftKneeRot = [ 166 leftKneeRotStart[0] + frameFraction * (leftKneeRotEnd[0] - leftKneeRotStart[0]) 167 ] 168 self._leftKneeVel = [(leftKneeRotEnd[0] - leftKneeRotStart[0]) / keyFrameDuration] 169 170 leftAnkleRotStart = [frameData[36], frameData[37], frameData[38], frameData[35]] 171 leftAnkleRotEnd = [frameDataNext[36], frameDataNext[37], frameDataNext[38], frameDataNext[35]] 172 self._leftAnkleRot = bullet_client.getQuaternionSlerp(leftAnkleRotStart, leftAnkleRotEnd, 173 frameFraction) 174 self._leftAnkleVel = self.ComputeAngVel(leftAnkleRotStart, leftAnkleRotEnd, keyFrameDuration, 175 bullet_client) 176 177 leftShoulderRotStart = [frameData[40], frameData[41], frameData[42], frameData[39]] 178 leftShoulderRotEnd = [ 179 frameDataNext[40], frameDataNext[41], frameDataNext[42], frameDataNext[39] 180 ] 181 self._leftShoulderRot = bullet_client.getQuaternionSlerp(leftShoulderRotStart, 182 leftShoulderRotEnd, frameFraction) 183 self._leftShoulderVel = self.ComputeAngVel(leftShoulderRotStart, leftShoulderRotEnd, 184 keyFrameDuration, bullet_client) 185 186 leftElbowRotStart = [frameData[43]] 187 leftElbowRotEnd = [frameDataNext[43]] 188 self._leftElbowRot = [ 189 leftElbowRotStart[0] + frameFraction * (leftElbowRotEnd[0] - leftElbowRotStart[0]) 190 ] 191 self._leftElbowVel = [(leftElbowRotEnd[0] - leftElbowRotStart[0]) / keyFrameDuration] 192 193 194class Humanoid(object): 195 196 def __init__(self, pybullet_client, motion_data, baseShift): 197 """Constructs a humanoid and reset it to the initial states. 198 Args: 199 pybullet_client: The instance of BulletClient to manage different 200 simulations. 201 """ 202 self._baseShift = baseShift 203 self._pybullet_client = pybullet_client 204 205 self.kin_client = BulletClient( 206 pybullet_client.DIRECT 207 ) # use SHARED_MEMORY for visual debugging, start a GUI physics server first 208 self.kin_client.resetSimulation() 209 self.kin_client.setAdditionalSearchPath(pybullet_data.getDataPath()) 210 self.kin_client.configureDebugVisualizer(self.kin_client.COV_ENABLE_Y_AXIS_UP, 1) 211 self.kin_client.setGravity(0, -9.8, 0) 212 213 self._motion_data = motion_data 214 print("LOADING humanoid!") 215 self._humanoid = self._pybullet_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0], 216 globalScaling=0.25, 217 useFixedBase=False) 218 219 self._kinematicHumanoid = self.kin_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0], 220 globalScaling=0.25, 221 useFixedBase=False) 222 223 #print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid)) 224 pose = HumanoidPose() 225 226 for i in range(self._motion_data.NumFrames() - 1): 227 frameData = self._motion_data._motion_data['Frames'][i] 228 pose.PostProcessMotionData(frameData) 229 230 self._pybullet_client.resetBasePositionAndOrientation(self._humanoid, self._baseShift, 231 [0, 0, 0, 1]) 232 self._pybullet_client.changeDynamics(self._humanoid, -1, linearDamping=0, angularDamping=0) 233 for j in range(self._pybullet_client.getNumJoints(self._humanoid)): 234 ji = self._pybullet_client.getJointInfo(self._humanoid, j) 235 self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0) 236 self._pybullet_client.changeVisualShape(self._humanoid, j, rgbaColor=[1, 1, 1, 1]) 237 #print("joint[",j,"].type=",jointTypes[ji[2]]) 238 #print("joint[",j,"].name=",ji[1]) 239 240 self._initial_state = self._pybullet_client.saveState() 241 self._allowed_body_parts = [11, 14] 242 self.Reset() 243 244 def Reset(self): 245 self._pybullet_client.restoreState(self._initial_state) 246 self.SetSimTime(0) 247 pose = self.InitializePoseFromMotionData() 248 self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client) 249 250 def RenderReference(self, t): 251 self.SetSimTime(t) 252 frameData = self._motion_data._motion_data['Frames'][self._frame] 253 frameDataNext = self._motion_data._motion_data['Frames'][self._frameNext] 254 pose = HumanoidPose() 255 pose.Slerp(self._frameFraction, frameData, frameDataNext, self._pybullet_client) 256 self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client) 257 258 def CalcCycleCount(self, simTime, cycleTime): 259 phases = simTime / cycleTime 260 count = math.floor(phases) 261 loop = True 262 #count = (loop) ? count : cMathUtil::Clamp(count, 0, 1); 263 return count 264 265 def SetSimTime(self, t): 266 self._simTime = t 267 #print("SetTimeTime time =",t) 268 keyFrameDuration = self._motion_data.KeyFrameDuraction() 269 cycleTime = keyFrameDuration * (self._motion_data.NumFrames() - 1) 270 #print("self._motion_data.NumFrames()=",self._motion_data.NumFrames()) 271 #print("cycleTime=",cycleTime) 272 cycles = self.CalcCycleCount(t, cycleTime) 273 #print("cycles=",cycles) 274 frameTime = t - cycles * cycleTime 275 if (frameTime < 0): 276 frameTime += cycleTime 277 278 #print("keyFrameDuration=",keyFrameDuration) 279 #print("frameTime=",frameTime) 280 self._frame = int(frameTime / keyFrameDuration) 281 #print("self._frame=",self._frame) 282 283 self._frameNext = self._frame + 1 284 if (self._frameNext >= self._motion_data.NumFrames()): 285 self._frameNext = self._frame 286 287 self._frameFraction = (frameTime - self._frame * keyFrameDuration) / (keyFrameDuration) 288 #print("self._frameFraction=",self._frameFraction) 289 290 def Terminates(self): 291 #check if any non-allowed body part hits the ground 292 terminates = False 293 pts = self._pybullet_client.getContactPoints() 294 for p in pts: 295 part = -1 296 if (p[1] == self._humanoid): 297 part = p[3] 298 if (p[2] == self._humanoid): 299 part = p[4] 300 if (part >= 0 and part not in self._allowed_body_parts): 301 terminates = True 302 303 return terminates 304 305 def BuildHeadingTrans(self, rootOrn): 306 #align root transform 'forward' with world-space x axis 307 eul = self._pybullet_client.getEulerFromQuaternion(rootOrn) 308 refDir = [1, 0, 0] 309 rotVec = self._pybullet_client.rotateVector(rootOrn, refDir) 310 heading = math.atan2(-rotVec[2], rotVec[0]) 311 heading2 = eul[1] 312 #print("heading=",heading) 313 headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0, 1, 0], -heading) 314 return headingOrn 315 316 def GetPhase(self): 317 keyFrameDuration = self._motion_data.KeyFrameDuraction() 318 cycleTime = keyFrameDuration * (self._motion_data.NumFrames() - 1) 319 phase = self._simTime / cycleTime 320 phase = math.fmod(phase, 1.0) 321 if (phase < 0): 322 phase += 1 323 return phase 324 325 def BuildOriginTrans(self): 326 rootPos, rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) 327 328 #print("rootPos=",rootPos, " rootOrn=",rootOrn) 329 invRootPos = [-rootPos[0], 0, -rootPos[2]] 330 #invOrigTransPos, invOrigTransOrn = self._pybullet_client.invertTransform(rootPos,rootOrn) 331 headingOrn = self.BuildHeadingTrans(rootOrn) 332 #print("headingOrn=",headingOrn) 333 headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn) 334 #print("headingMat=",headingMat) 335 #dummy, rootOrnWithoutHeading = self._pybullet_client.multiplyTransforms([0,0,0],headingOrn, [0,0,0], rootOrn) 336 #dummy, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0,0,0],rootOrnWithoutHeading, invOrigTransPos, invOrigTransOrn) 337 338 invOrigTransPos, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0, 0, 0], 339 headingOrn, 340 invRootPos, 341 [0, 0, 0, 1]) 342 #print("invOrigTransPos=",invOrigTransPos) 343 #print("invOrigTransOrn=",invOrigTransOrn) 344 invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn) 345 #print("invOrigTransMat =",invOrigTransMat ) 346 return invOrigTransPos, invOrigTransOrn 347 348 def InitializePoseFromMotionData(self): 349 frameData = self._motion_data._motion_data['Frames'][self._frame] 350 frameDataNext = self._motion_data._motion_data['Frames'][self._frameNext] 351 pose = HumanoidPose() 352 pose.Slerp(self._frameFraction, frameData, frameDataNext, self._pybullet_client) 353 return pose 354 355 def ApplyAction(self, action): 356 #turn action into pose 357 pose = HumanoidPose() 358 pose.Reset() 359 index = 0 360 angle = action[index] 361 axis = [action[index + 1], action[index + 2], action[index + 3]] 362 index += 4 363 pose._chestRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) 364 #print("pose._chestRot=",pose._chestRot) 365 366 angle = action[index] 367 axis = [action[index + 1], action[index + 2], action[index + 3]] 368 index += 4 369 pose._neckRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) 370 371 angle = action[index] 372 axis = [action[index + 1], action[index + 2], action[index + 3]] 373 index += 4 374 pose._rightHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) 375 376 angle = action[index] 377 index += 1 378 pose._rightKneeRot = [angle] 379 380 angle = action[index] 381 axis = [action[index + 1], action[index + 2], action[index + 3]] 382 index += 4 383 pose._rightAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) 384 385 angle = action[index] 386 axis = [action[index + 1], action[index + 2], action[index + 3]] 387 index += 4 388 pose._rightShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) 389 390 angle = action[index] 391 index += 1 392 pose._rightElbowRot = [angle] 393 394 angle = action[index] 395 axis = [action[index + 1], action[index + 2], action[index + 3]] 396 index += 4 397 pose._leftHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) 398 399 angle = action[index] 400 index += 1 401 pose._leftKneeRot = [angle] 402 403 angle = action[index] 404 axis = [action[index + 1], action[index + 2], action[index + 3]] 405 index += 4 406 pose._leftAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) 407 408 angle = action[index] 409 axis = [action[index + 1], action[index + 2], action[index + 3]] 410 index += 4 411 pose._leftShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) 412 413 angle = action[index] 414 index += 1 415 pose._leftElbowRot = [angle] 416 417 #print("index=",index) 418 419 initializeBase = False 420 initializeVelocities = False 421 self.ApplyPose(pose, initializeBase, initializeVelocities, self._humanoid, 422 self._pybullet_client) 423 424 def ApplyPose(self, pose, initializeBase, initializeVelocities, humanoid, bc): 425 #todo: get tunable parametes from a json file or from URDF (kd, maxForce) 426 if (initializeBase): 427 bc.changeVisualShape(humanoid, 2, rgbaColor=[1, 0, 0, 1]) 428 basePos = [ 429 pose._basePos[0] + self._baseShift[0], pose._basePos[1] + self._baseShift[1], 430 pose._basePos[2] + self._baseShift[2] 431 ] 432 433 bc.resetBasePositionAndOrientation(humanoid, basePos, pose._baseOrn) 434 if initializeVelocities: 435 bc.resetBaseVelocity(humanoid, pose._baseLinVel, pose._baseAngVel) 436 #print("resetBaseVelocity=",pose._baseLinVel) 437 else: 438 bc.changeVisualShape(humanoid, 2, rgbaColor=[1, 1, 1, 1]) 439 440 kp = 0.03 441 chest = 1 442 neck = 2 443 rightShoulder = 3 444 rightElbow = 4 445 leftShoulder = 6 446 leftElbow = 7 447 rightHip = 9 448 rightKnee = 10 449 rightAnkle = 11 450 leftHip = 12 451 leftKnee = 13 452 leftAnkle = 14 453 controlMode = bc.POSITION_CONTROL 454 455 if (initializeBase): 456 if initializeVelocities: 457 bc.resetJointStateMultiDof(humanoid, chest, pose._chestRot, pose._chestVel) 458 bc.resetJointStateMultiDof(humanoid, neck, pose._neckRot, pose._neckVel) 459 bc.resetJointStateMultiDof(humanoid, rightHip, pose._rightHipRot, pose._rightHipVel) 460 bc.resetJointStateMultiDof(humanoid, rightKnee, pose._rightKneeRot, pose._rightKneeVel) 461 bc.resetJointStateMultiDof(humanoid, rightAnkle, pose._rightAnkleRot, pose._rightAnkleVel) 462 bc.resetJointStateMultiDof(humanoid, rightShoulder, pose._rightShoulderRot, 463 pose._rightShoulderVel) 464 bc.resetJointStateMultiDof(humanoid, rightElbow, pose._rightElbowRot, pose._rightElbowVel) 465 bc.resetJointStateMultiDof(humanoid, leftHip, pose._leftHipRot, pose._leftHipVel) 466 bc.resetJointStateMultiDof(humanoid, leftKnee, pose._leftKneeRot, pose._leftKneeVel) 467 bc.resetJointStateMultiDof(humanoid, leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel) 468 bc.resetJointStateMultiDof(humanoid, leftShoulder, pose._leftShoulderRot, 469 pose._leftShoulderVel) 470 bc.resetJointStateMultiDof(humanoid, leftElbow, pose._leftElbowRot, pose._leftElbowVel) 471 else: 472 bc.resetJointStateMultiDof(humanoid, chest, pose._chestRot) 473 bc.resetJointStateMultiDof(humanoid, neck, pose._neckRot) 474 bc.resetJointStateMultiDof(humanoid, rightHip, pose._rightHipRot) 475 bc.resetJointStateMultiDof(humanoid, rightKnee, pose._rightKneeRot) 476 bc.resetJointStateMultiDof(humanoid, rightAnkle, pose._rightAnkleRot) 477 bc.resetJointStateMultiDof(humanoid, rightShoulder, pose._rightShoulderRot) 478 bc.resetJointStateMultiDof(humanoid, rightElbow, pose._rightElbowRot) 479 bc.resetJointStateMultiDof(humanoid, leftHip, pose._leftHipRot) 480 bc.resetJointStateMultiDof(humanoid, leftKnee, pose._leftKneeRot) 481 bc.resetJointStateMultiDof(humanoid, leftAnkle, pose._leftAnkleRot) 482 bc.resetJointStateMultiDof(humanoid, leftShoulder, pose._leftShoulderRot) 483 bc.resetJointStateMultiDof(humanoid, leftElbow, pose._leftElbowRot) 484 485 bc.setJointMotorControlMultiDof(humanoid, 486 chest, 487 controlMode, 488 targetPosition=pose._chestRot, 489 positionGain=kp, 490 force=[200]) 491 bc.setJointMotorControlMultiDof(humanoid, 492 neck, 493 controlMode, 494 targetPosition=pose._neckRot, 495 positionGain=kp, 496 force=[50]) 497 bc.setJointMotorControlMultiDof(humanoid, 498 rightHip, 499 controlMode, 500 targetPosition=pose._rightHipRot, 501 positionGain=kp, 502 force=[200]) 503 bc.setJointMotorControlMultiDof(humanoid, 504 rightKnee, 505 controlMode, 506 targetPosition=pose._rightKneeRot, 507 positionGain=kp, 508 force=[150]) 509 bc.setJointMotorControlMultiDof(humanoid, 510 rightAnkle, 511 controlMode, 512 targetPosition=pose._rightAnkleRot, 513 positionGain=kp, 514 force=[90]) 515 bc.setJointMotorControlMultiDof(humanoid, 516 rightShoulder, 517 controlMode, 518 targetPosition=pose._rightShoulderRot, 519 positionGain=kp, 520 force=[100]) 521 bc.setJointMotorControlMultiDof(humanoid, 522 rightElbow, 523 controlMode, 524 targetPosition=pose._rightElbowRot, 525 positionGain=kp, 526 force=[60]) 527 bc.setJointMotorControlMultiDof(humanoid, 528 leftHip, 529 controlMode, 530 targetPosition=pose._leftHipRot, 531 positionGain=kp, 532 force=[200]) 533 bc.setJointMotorControlMultiDof(humanoid, 534 leftKnee, 535 controlMode, 536 targetPosition=pose._leftKneeRot, 537 positionGain=kp, 538 force=[150]) 539 bc.setJointMotorControlMultiDof(humanoid, 540 leftAnkle, 541 controlMode, 542 targetPosition=pose._leftAnkleRot, 543 positionGain=kp, 544 force=[90]) 545 bc.setJointMotorControlMultiDof(humanoid, 546 leftShoulder, 547 controlMode, 548 targetPosition=pose._leftShoulderRot, 549 positionGain=kp, 550 force=[100]) 551 bc.setJointMotorControlMultiDof(humanoid, 552 leftElbow, 553 controlMode, 554 targetPosition=pose._leftElbowRot, 555 positionGain=kp, 556 force=[60]) 557 558 #debug space 559 #if (False): 560 # for j in range (bc.getNumJoints(self._humanoid)): 561 # js = bc.getJointState(self._humanoid, j) 562 # bc.resetJointState(self._humanoidDebug, j,js[0]) 563 # jsm = bc.getJointStateMultiDof(self._humanoid, j) 564 # if (len(jsm[0])>0): 565 # bc.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0]) 566 567 def GetState(self): 568 569 stateVector = [] 570 phase = self.GetPhase() 571 #print("phase=",phase) 572 stateVector.append(phase) 573 574 rootTransPos, rootTransOrn = self.BuildOriginTrans() 575 basePos, baseOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) 576 577 rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, 578 basePos, [0, 0, 0, 1]) 579 #print("!!!rootPosRel =",rootPosRel ) 580 #print("rootTransPos=",rootTransPos) 581 #print("basePos=",basePos) 582 localPos, localOrn = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, 583 basePos, baseOrn) 584 585 localPos = [ 586 localPos[0] - rootPosRel[0], localPos[1] - rootPosRel[1], localPos[2] - rootPosRel[2] 587 ] 588 #print("localPos=",localPos) 589 590 stateVector.append(rootPosRel[1]) 591 592 self.pb2dmJoints = [0, 1, 2, 9, 10, 11, 3, 4, 5, 12, 13, 14, 6, 7, 8] 593 594 for pbJoint in range(self._pybullet_client.getNumJoints(self._humanoid)): 595 j = self.pb2dmJoints[pbJoint] 596 #print("joint order:",j) 597 ls = self._pybullet_client.getLinkState(self._humanoid, j, computeForwardKinematics=True) 598 linkPos = ls[0] 599 linkOrn = ls[1] 600 linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms( 601 rootTransPos, rootTransOrn, linkPos, linkOrn) 602 if (linkOrnLocal[3] < 0): 603 linkOrnLocal = [-linkOrnLocal[0], -linkOrnLocal[1], -linkOrnLocal[2], -linkOrnLocal[3]] 604 linkPosLocal = [ 605 linkPosLocal[0] - rootPosRel[0], linkPosLocal[1] - rootPosRel[1], 606 linkPosLocal[2] - rootPosRel[2] 607 ] 608 for l in linkPosLocal: 609 stateVector.append(l) 610 611 #re-order the quaternion, DeepMimic uses w,x,y,z 612 stateVector.append(linkOrnLocal[3]) 613 stateVector.append(linkOrnLocal[0]) 614 stateVector.append(linkOrnLocal[1]) 615 stateVector.append(linkOrnLocal[2]) 616 617 for pbJoint in range(self._pybullet_client.getNumJoints(self._humanoid)): 618 j = self.pb2dmJoints[pbJoint] 619 ls = self._pybullet_client.getLinkState(self._humanoid, j, computeLinkVelocity=True) 620 linkLinVel = ls[6] 621 linkAngVel = ls[7] 622 for l in linkLinVel: 623 stateVector.append(l) 624 for l in linkAngVel: 625 stateVector.append(l) 626 627 #print("stateVector len=",len(stateVector)) 628 #for st in range (len(stateVector)): 629 # print("state[",st,"]=",stateVector[st]) 630 return stateVector 631 632 def GetReward(self): 633 #from DeepMimic double cSceneImitate::CalcRewardImitate 634 pose_w = 0.5 635 vel_w = 0.05 636 end_eff_w = 0 #0.15 637 root_w = 0 #0.2 638 com_w = 0.1 639 640 total_w = pose_w + vel_w + end_eff_w + root_w + com_w 641 pose_w /= total_w 642 vel_w /= total_w 643 end_eff_w /= total_w 644 root_w /= total_w 645 com_w /= total_w 646 647 pose_scale = 2 648 vel_scale = 0.1 649 end_eff_scale = 40 650 root_scale = 5 651 com_scale = 10 652 err_scale = 1 653 654 reward = 0 655 656 pose_err = 0 657 vel_err = 0 658 end_eff_err = 0 659 root_err = 0 660 com_err = 0 661 heading_err = 0 662 663 #create a mimic reward, comparing the dynamics humanoid with a kinematic one 664 665 pose = self.InitializePoseFromMotionData() 666 #print("self._kinematicHumanoid=",self._kinematicHumanoid) 667 #print("kinematicHumanoid #joints=",self.kin_client.getNumJoints(self._kinematicHumanoid)) 668 self.ApplyPose(pose, True, True, self._kinematicHumanoid, self.kin_client) 669 670 #const Eigen::VectorXd& pose0 = sim_char.GetPose(); 671 #const Eigen::VectorXd& vel0 = sim_char.GetVel(); 672 #const Eigen::VectorXd& pose1 = kin_char.GetPose(); 673 #const Eigen::VectorXd& vel1 = kin_char.GetVel(); 674 #tMatrix origin_trans = sim_char.BuildOriginTrans(); 675 #tMatrix kin_origin_trans = kin_char.BuildOriginTrans(); 676 # 677 #tVector com0_world = sim_char.CalcCOM(); 678 #tVector com_vel0_world = sim_char.CalcCOMVel(); 679 #tVector com1_world; 680 #tVector com_vel1_world; 681 #cRBDUtil::CalcCoM(joint_mat, body_defs, pose1, vel1, com1_world, com_vel1_world); 682 # 683 root_id = 0 684 #tVector root_pos0 = cKinTree::GetRootPos(joint_mat, pose0); 685 #tVector root_pos1 = cKinTree::GetRootPos(joint_mat, pose1); 686 #tQuaternion root_rot0 = cKinTree::GetRootRot(joint_mat, pose0); 687 #tQuaternion root_rot1 = cKinTree::GetRootRot(joint_mat, pose1); 688 #tVector root_vel0 = cKinTree::GetRootVel(joint_mat, vel0); 689 #tVector root_vel1 = cKinTree::GetRootVel(joint_mat, vel1); 690 #tVector root_ang_vel0 = cKinTree::GetRootAngVel(joint_mat, vel0); 691 #tVector root_ang_vel1 = cKinTree::GetRootAngVel(joint_mat, vel1); 692 693 mJointWeights = [ 694 0.20833, 0.10416, 0.0625, 0.10416, 0.0625, 0.041666666666666671, 0.0625, 0.0416, 0.00, 695 0.10416, 0.0625, 0.0416, 0.0625, 0.0416, 0.0000 696 ] 697 698 num_end_effs = 0 699 num_joints = 15 700 701 root_rot_w = mJointWeights[root_id] 702 #pose_err += root_rot_w * cKinTree::CalcRootRotErr(joint_mat, pose0, pose1) 703 #vel_err += root_rot_w * cKinTree::CalcRootAngVelErr(joint_mat, vel0, vel1) 704 705 for j in range(num_joints): 706 curr_pose_err = 0 707 curr_vel_err = 0 708 w = mJointWeights[j] 709 710 simJointInfo = self._pybullet_client.getJointStateMultiDof(self._humanoid, j) 711 712 #print("simJointInfo.pos=",simJointInfo[0]) 713 #print("simJointInfo.vel=",simJointInfo[1]) 714 kinJointInfo = self.kin_client.getJointStateMultiDof(self._kinematicHumanoid, j) 715 #print("kinJointInfo.pos=",kinJointInfo[0]) 716 #print("kinJointInfo.vel=",kinJointInfo[1]) 717 if (len(simJointInfo[0]) == 1): 718 angle = simJointInfo[0][0] - kinJointInfo[0][0] 719 curr_pose_err = angle * angle 720 velDiff = simJointInfo[1][0] - kinJointInfo[1][0] 721 curr_vel_err = velDiff * velDiff 722 if (len(simJointInfo[0]) == 4): 723 #print("quaternion diff") 724 diffQuat = self._pybullet_client.getDifferenceQuaternion(simJointInfo[0], kinJointInfo[0]) 725 axis, angle = self._pybullet_client.getAxisAngleFromQuaternion(diffQuat) 726 curr_pose_err = angle * angle 727 diffVel = [ 728 simJointInfo[1][0] - kinJointInfo[1][0], simJointInfo[1][1] - kinJointInfo[1][1], 729 simJointInfo[1][2] - kinJointInfo[1][2] 730 ] 731 curr_vel_err = diffVel[0] * diffVel[0] + diffVel[1] * diffVel[1] + diffVel[2] * diffVel[2] 732 733 pose_err += w * curr_pose_err 734 vel_err += w * curr_vel_err 735 736 # bool is_end_eff = sim_char.IsEndEffector(j) 737 # if (is_end_eff) 738 # { 739 # tVector pos0 = sim_char.CalcJointPos(j) 740 # tVector pos1 = cKinTree::CalcJointWorldPos(joint_mat, pose1, j) 741 # double ground_h0 = mGround->SampleHeight(pos0) 742 # double ground_h1 = kin_char.GetOriginPos()[1] 743 # 744 # tVector pos_rel0 = pos0 - root_pos0 745 # tVector pos_rel1 = pos1 - root_pos1 746 # pos_rel0[1] = pos0[1] - ground_h0 747 # pos_rel1[1] = pos1[1] - ground_h1 748 # 749 # pos_rel0 = origin_trans * pos_rel0 750 # pos_rel1 = kin_origin_trans * pos_rel1 751 # 752 # curr_end_err = (pos_rel1 - pos_rel0).squaredNorm() 753 # end_eff_err += curr_end_err; 754 # ++num_end_effs; 755 # } 756 #} 757 #if (num_end_effs > 0): 758 # end_eff_err /= num_end_effs 759 # 760 #double root_ground_h0 = mGround->SampleHeight(sim_char.GetRootPos()) 761 #double root_ground_h1 = kin_char.GetOriginPos()[1] 762 #root_pos0[1] -= root_ground_h0 763 #root_pos1[1] -= root_ground_h1 764 #root_pos_err = (root_pos0 - root_pos1).squaredNorm() 765 # 766 #root_rot_err = cMathUtil::QuatDiffTheta(root_rot0, root_rot1) 767 #root_rot_err *= root_rot_err 768 769 #root_vel_err = (root_vel1 - root_vel0).squaredNorm() 770 #root_ang_vel_err = (root_ang_vel1 - root_ang_vel0).squaredNorm() 771 772 #root_err = root_pos_err 773 # + 0.1 * root_rot_err 774 # + 0.01 * root_vel_err 775 # + 0.001 * root_ang_vel_err 776 #com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm() 777 778 #print("pose_err=",pose_err) 779 #print("vel_err=",vel_err) 780 pose_reward = math.exp(-err_scale * pose_scale * pose_err) 781 vel_reward = math.exp(-err_scale * vel_scale * vel_err) 782 end_eff_reward = math.exp(-err_scale * end_eff_scale * end_eff_err) 783 root_reward = math.exp(-err_scale * root_scale * root_err) 784 com_reward = math.exp(-err_scale * com_scale * com_err) 785 786 reward = pose_w * pose_reward + vel_w * vel_reward + end_eff_w * end_eff_reward + root_w * root_reward + com_w * com_reward 787 788 #print("reward = %f (pose_reward=%f, vel_reward=%f, end_eff_reward=%f, root_reward=%f, com_reward=%f)\n", reward, 789 # pose_reward,vel_reward,end_eff_reward, root_reward, com_reward); 790 791 return reward 792 793 def GetBasePosition(self): 794 pos, orn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) 795 return pos 796