def applyActions(self, actions, timeStep=1./240, frameDeltaTime=0.0625):
'''
Action indices should correspond to the following:
[0-2] - right_hip Type: SPHERICAL
'''
# Format actions from -1,1 to actual values.
# New scaledAction values will fall in range of -maxForce, maxForce
maxForce = 200
scaledActions = [action*maxForce for action in actions]
formattedActions = []
# condense flat array into list of list format for spherical joint control
for i in [0]:
formattedActions.append(scaledActions[i:i+3])
numSubFrames = int(frameDeltaTime / timeStep)
for _ in range(numSubFrames):
# Set spherical joint torques
p.setJointMotorControlMultiDofArray(
bodyUniqueId=self.humanoidAgent,
jointIndices=self.sphericalJoints,
controlMode=p.TORQUE_CONTROL,
forces=formattedActions[:]
)
p.stepSimulation()
def move(self):
p.setJointMotorControlMultiDofArray(
self.id,
self.jointIndices,
p.POSITION_CONTROL,
targetPositions=self.getSinTargetPositions())
p.stepSimulation()
def ControlSpherJoints(self):
p.setJointMotorControlMultiDofArray(
self.linkage,
self.spherJointIds,
self.spherJointControl,
targetPositions=[self.spherJointPos] * self.spherJointNum,
targetVelocities=[self.spherJointVelo] * self.spherJointNum,
forces=[self.spherJointForce] * self.spherJointNum,
positionGains=[self.spherJointKp] * self.spherJointNum,
velocityGains=[self.spherJointKv] * self.spherJointNum)
def applyActions(self, actions):
'''
Action indices should correspond to the following:
[0-2] - chest Type: SPHERICAL
[3-5] - neck Type: SPHERICAL
[6-8] - right_hip Type: SPHERICAL
[9-11] - right_ankle Type: SPHERICAL
[12-14] - right_shoulder Type: SPHERICAL
[15-17] - left_hip Type: SPHERICAL
[18-20] - left_ankle Type: SPHERICAL
[21-23] - left_shoulder Type: SPHERICAL
[24] - right_knee Type: REVOLUTE
[25] - right_elbow Type: REVOLUTE
[26] - left_knee Type: REVOLUTE
[27] - left_elbow Type: REVOLUTE
'''
# Format actions from -1,1 to actual values.
# New scaledAction values will fall in range of -maxForce, maxForce
maxForce = 10
scaledActions = [action*maxForce for action in actions]
formattedActions = []
# condense flat array into list of list format for spherical joint control
for i in [0,3,6,9,12,15,18,21]:
formattedActions.append(scaledActions[i:i+3])
formattedActions.extend(scaledActions[24:])
# Set spherical joint torques
p.setJointMotorControlMultiDofArray(
bodyUniqueId=self.humanoidAgent,
jointIndices=self.sphericalJoints,
controlMode=p.TORQUE_CONTROL,
forces=formattedActions[:8]
)
# Set revolute joint torques (Commands are different)
p.setJointMotorControlArray(
bodyUniqueId=self.humanoidAgent,
jointIndices=self.revoluteJoints,
controlMode=p.TORQUE_CONTROL,
forces=formattedActions[8:]
)
def pdControlMove(self,
pd_func,
targetPositions=None,
targetVelocities=None):
numJoints = p.getNumJoints(self.id)
if targetPositions is None: # default sin move
targetPositions = []
for pos in self.getSinTargetPositions():
axis, ang = p.getAxisAngleFromQuaternion(pos)
targetPositions.append(np.array(axis) * ang)
if targetVelocities is None:
targetVelocities = np.full((numJoints, 3), 0.)
curPositions = []
curVelocities = []
for jointState in p.getJointStatesMultiDof(self.id, self.jointIndices):
state = list(jointState)
axis, ang = p.getAxisAngleFromQuaternion(state[0])
curPositions.append(np.array(ang * np.array(axis)))
curVelocities.append(np.array(state[1]))
torques = []
for i in range(numJoints):
torques.append(
pd_func(curPositions[i], curVelocities[i], targetPositions[i],
targetVelocities[i]))
p.setJointMotorControlMultiDofArray(self.id,
self.jointIndices,
p.TORQUE_CONTROL,
forces=torques)
p.stepSimulation()
leftHipRot, leftKneeRot, leftAnkleRot,
leftShoulderRot, leftElbowRot]
maxForces = [ [maxForce,maxForce,maxForce], [maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce],
[maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce],
[maxForce,maxForce,maxForce], [maxForce], [maxForce,maxForce,maxForce],
[maxForce,maxForce,maxForce], [maxForce]]
kps = [1000]*12
kds = [100]*12
p.setJointMotorControlMultiDofArray(humanoid,
indices,
p.STABLE_PD_CONTROL,
targetPositions=targetPositions,
positionGains=kps,
velocityGains=kds,
forces=maxForces)
taus3 = explicitPD.computePD(bodyUniqueId=humanoid3,
jointIndices=jointIndicesAll,
desiredPositions=pose,
desiredVelocities=[0] * totalDofs,
kps=kpOrg,
kds=kdOrg,
maxForces=[maxForce * 0.05] * totalDofs,
timeStep=timeStep)
#taus=[0]*43
dofIndex = 7