def _setup_bullet_client(self, client: bc.BulletClient):
client.resetSimulation()
client.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
# PyBullet defaults the timestep to 240Hz. Several parameters are tuned with
# this value in mind. For example the number of solver iterations and the error
# reduction parameters (erp) for contact, friction and non-contact joints.
# Attempting to get around this we set the number of substeps so that
# timestep * substeps = 240Hz. Bullet (C++) does something to this effect as
# well (https://git.io/Jvf0M), but PyBullet does not expose it.
client.setPhysicsEngineParameter(
fixedTimeStep=self._timestep_sec,
numSubSteps=math.ceil(self._timestep_sec / (1 / 240)),
)
client.setGravity(0, 0, -9.8)
plane_path = self._scenario.plane_filepath
# 1e6 is the default value for plane length and width.
plane_scale = (max(self._scenario.map_bounding_box[0],
self._scenario.map_bounding_box[1]) / 1e6)
if not os.path.exists(plane_path):
with pkg_resources.path(models, "plane.urdf") as path:
plane_path = str(path.absolute())
self._ground_bullet_id = client.loadURDF(
plane_path,
useFixedBase=True,
basePosition=self._scenario.map_bounding_box[2],
globalScaling=1.1 * plane_scale,
)
def _bullet_connect(self, render: bool) -> BulletClient:
if render:
bullet_client = BulletClient(connection_mode=pybullet.GUI)
bullet_client.configureDebugVisualizer(
bullet_client.COV_ENABLE_Y_AXIS_UP, 1)
else:
bullet_client = BulletClient(connection_mode=pybullet.DIRECT)
return bullet_client
def main(args):
"""Test GUI application."""
client = BulletClient(pb.GUI)
client.setGravity(0, 0, -10)
client.resetDebugVisualizerCamera(cameraDistance=0.5,
cameraYaw=-40.0,
cameraPitch=-40.0,
cameraTargetPosition=[0.0, 0.0, 0.0])
if args.dofs == 20:
hand_model = HandModel20(left_hand=args.left_hand)
elif args.dofs == 45:
hand_model = HandModel45(left_hand=args.left_hand)
else:
raise ValueError('Only 20 and 45 DoF models are supported.')
flags = sum([
HandBody.FLAG_ENABLE_COLLISION_SHAPES,
HandBody.FLAG_ENABLE_VISUAL_SHAPES * args.visual,
HandBody.FLAG_JOINT_LIMITS, HandBody.FLAG_DYNAMICS,
HandBody.FLAG_USE_SELF_COLLISION * args.self_collisions
])
client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
hand = HandBody(client, hand_model, flags=flags)
client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
slider_ids = []
for coord in ('X', 'Y', 'Z'):
uid = client.addUserDebugParameter(f'base_{coord}', -0.5, 0.5, 0)
slider_ids.append(uid)
for coord in ('R', 'P', 'Y'):
uid = client.addUserDebugParameter(f'base_{coord}', -3.14, 3.14, 0)
slider_ids.append(uid)
for i, joint in enumerate(hand_model.joints):
name = hand_model.link_names[i]
for axis, (lower, upper) in zip(joint.axes, joint.limits):
uid = client.addUserDebugParameter(f'{name}[{axis}]', lower, upper,
0)
slider_ids.append(uid)
client.setRealTimeSimulation(True)
try:
while client.isConnected():
values = [client.readUserDebugParameter(uid) for uid in slider_ids]
position = values[0:3]
rotation = client.getQuaternionFromEuler(values[3:6])
angles = values[6:]
hand.set_target(position, rotation, angles)
except pb.error as err:
if str(err) not in [
'Not connected to physics server.', 'Failed to read parameter.'
]:
raise
def config_bullet(self):
if self.configs['gui']:
bullet_client = BulletClient(connection_mode=pybullet.GUI)
else:
bullet_client = BulletClient(connection_mode=pybullet.DIRECT)
# enable planar reflection
bullet_client.configureDebugVisualizer(
bullet_client.COV_ENABLE_PLANAR_REFLECTION, 1)
# enable shadows
bullet_client.configureDebugVisualizer(
bullet_client.COV_ENABLE_SHADOWS, 0)
if self.configs['debug']:
bullet_client.configureDebugVisualizer(
bullet_client.COV_ENABLE_GUI, 1)
else:
bullet_client.configureDebugVisualizer(
bullet_client.COV_ENABLE_GUI, 0)
bullet_client.setGravity(0, 0, -self.configs['physics']['gravity'])
bullet_client.setPhysicsEngineParameter(
fixedTimeStep=self.configs['physics']['timestep'],
contactERP=self.configs['physics']['contact_erp'])
return bullet_client
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.kin_client = BulletClient(
pybullet_client.DIRECT
) # use SHARED_MEMORY for visual debugging, start a GUI physics server first
self.kin_client.resetSimulation()
self.kin_client.setAdditionalSearchPath(pybullet_data.getDataPath())
self.kin_client.configureDebugVisualizer(self.kin_client.COV_ENABLE_Y_AXIS_UP, 1)
self.kin_client.setGravity(0, -9.8, 0)
self._motion_data = motion_data
print("LOADING humanoid!")
self._humanoid = self._pybullet_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0],
globalScaling=0.25,
useFixedBase=False)
self._kinematicHumanoid = self.kin_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0],
globalScaling=0.25,
useFixedBase=False)
#print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid))
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)):
ji = self._pybullet_client.getJointInfo(self._humanoid, j)
self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0)
self._pybullet_client.changeVisualShape(self._humanoid, j, rgbaColor=[1, 1, 1, 1])
#print("joint[",j,"].type=",jointTypes[ji[2]])
#print("joint[",j,"].name=",ji[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.SetSimTime(0)
pose = self.InitializePoseFromMotionData()
self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client)
def RenderReference(self, t):
self.SetSimTime(t)
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)
self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client)
def CalcCycleCount(self, simTime, cycleTime):
phases = simTime / cycleTime
count = math.floor(phases)
loop = True
#count = (loop) ? count : cMathUtil::Clamp(count, 0, 1);
return count
def SetSimTime(self, t):
self._simTime = t
#print("SetTimeTime time =",t)
keyFrameDuration = self._motion_data.KeyFrameDuraction()
cycleTime = keyFrameDuration * (self._motion_data.NumFrames() - 1)
#print("self._motion_data.NumFrames()=",self._motion_data.NumFrames())
#print("cycleTime=",cycleTime)
cycles = self.CalcCycleCount(t, cycleTime)
#print("cycles=",cycles)
frameTime = t - cycles * cycleTime
if (frameTime < 0):
frameTime += cycleTime
#print("keyFrameDuration=",keyFrameDuration)
#print("frameTime=",frameTime)
self._frame = int(frameTime / keyFrameDuration)
#print("self._frame=",self._frame)
self._frameNext = self._frame + 1
if (self._frameNext >= self._motion_data.NumFrames()):
self._frameNext = self._frame
self._frameFraction = (frameTime - self._frame * keyFrameDuration) / (keyFrameDuration)
#print("self._frameFraction=",self._frameFraction)
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 >= 0 and part not in self._allowed_body_parts):
terminates = True
return terminates
def BuildHeadingTrans(self, rootOrn):
#align root transform 'forward' with world-space x axis
eul = self._pybullet_client.getEulerFromQuaternion(rootOrn)
refDir = [1, 0, 0]
rotVec = self._pybullet_client.rotateVector(rootOrn, refDir)
heading = math.atan2(-rotVec[2], rotVec[0])
heading2 = eul[1]
#print("heading=",heading)
headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0, 1, 0], -heading)
return headingOrn
def GetPhase(self):
keyFrameDuration = self._motion_data.KeyFrameDuraction()
cycleTime = keyFrameDuration * (self._motion_data.NumFrames() - 1)
phase = self._simTime / cycleTime
phase = math.fmod(phase, 1.0)
if (phase < 0):
phase += 1
return phase
def BuildOriginTrans(self):
rootPos, rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
#print("rootPos=",rootPos, " rootOrn=",rootOrn)
invRootPos = [-rootPos[0], 0, -rootPos[2]]
#invOrigTransPos, invOrigTransOrn = self._pybullet_client.invertTransform(rootPos,rootOrn)
headingOrn = self.BuildHeadingTrans(rootOrn)
#print("headingOrn=",headingOrn)
headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn)
#print("headingMat=",headingMat)
#dummy, rootOrnWithoutHeading = self._pybullet_client.multiplyTransforms([0,0,0],headingOrn, [0,0,0], rootOrn)
#dummy, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0,0,0],rootOrnWithoutHeading, invOrigTransPos, invOrigTransOrn)
invOrigTransPos, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0, 0, 0],
headingOrn,
invRootPos,
[0, 0, 0, 1])
#print("invOrigTransPos=",invOrigTransPos)
#print("invOrigTransOrn=",invOrigTransOrn)
invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn)
#print("invOrigTransMat =",invOrigTransMat )
return invOrigTransPos, invOrigTransOrn
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()
pose.Reset()
index = 0
angle = action[index]
axis = [action[index + 1], action[index + 2], action[index + 3]]
index += 4
pose._chestRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
#print("pose._chestRot=",pose._chestRot)
angle = action[index]
axis = [action[index + 1], action[index + 2], action[index + 3]]
index += 4
pose._neckRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
angle = action[index]
axis = [action[index + 1], action[index + 2], action[index + 3]]
index += 4
pose._rightHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
angle = action[index]
index += 1
pose._rightKneeRot = [angle]
angle = action[index]
axis = [action[index + 1], action[index + 2], action[index + 3]]
index += 4
pose._rightAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
angle = action[index]
axis = [action[index + 1], action[index + 2], action[index + 3]]
index += 4
pose._rightShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
angle = action[index]
index += 1
pose._rightElbowRot = [angle]
angle = action[index]
axis = [action[index + 1], action[index + 2], action[index + 3]]
index += 4
pose._leftHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
angle = action[index]
index += 1
pose._leftKneeRot = [angle]
angle = action[index]
axis = [action[index + 1], action[index + 2], action[index + 3]]
index += 4
pose._leftAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
angle = action[index]
axis = [action[index + 1], action[index + 2], action[index + 3]]
index += 4
pose._leftShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle)
angle = action[index]
index += 1
pose._leftElbowRot = [angle]
#print("index=",index)
initializeBase = False
initializeVelocities = False
self.ApplyPose(pose, initializeBase, initializeVelocities, self._humanoid,
self._pybullet_client)
def ApplyPose(self, pose, initializeBase, initializeVelocities, humanoid, bc):
#todo: get tunable parametes from a json file or from URDF (kd, maxForce)
if (initializeBase):
bc.changeVisualShape(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]
]
bc.resetBasePositionAndOrientation(humanoid, basePos, pose._baseOrn)
if initializeVelocities:
bc.resetBaseVelocity(humanoid, pose._baseLinVel, pose._baseAngVel)
#print("resetBaseVelocity=",pose._baseLinVel)
else:
bc.changeVisualShape(humanoid, 2, rgbaColor=[1, 1, 1, 1])
kp = 0.03
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 = bc.POSITION_CONTROL
if (initializeBase):
if initializeVelocities:
bc.resetJointStateMultiDof(humanoid, chest, pose._chestRot, pose._chestVel)
bc.resetJointStateMultiDof(humanoid, neck, pose._neckRot, pose._neckVel)
bc.resetJointStateMultiDof(humanoid, rightHip, pose._rightHipRot, pose._rightHipVel)
bc.resetJointStateMultiDof(humanoid, rightKnee, pose._rightKneeRot, pose._rightKneeVel)
bc.resetJointStateMultiDof(humanoid, rightAnkle, pose._rightAnkleRot, pose._rightAnkleVel)
bc.resetJointStateMultiDof(humanoid, rightShoulder, pose._rightShoulderRot,
pose._rightShoulderVel)
bc.resetJointStateMultiDof(humanoid, rightElbow, pose._rightElbowRot, pose._rightElbowVel)
bc.resetJointStateMultiDof(humanoid, leftHip, pose._leftHipRot, pose._leftHipVel)
bc.resetJointStateMultiDof(humanoid, leftKnee, pose._leftKneeRot, pose._leftKneeVel)
bc.resetJointStateMultiDof(humanoid, leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel)
bc.resetJointStateMultiDof(humanoid, leftShoulder, pose._leftShoulderRot,
pose._leftShoulderVel)
bc.resetJointStateMultiDof(humanoid, leftElbow, pose._leftElbowRot, pose._leftElbowVel)
else:
bc.resetJointStateMultiDof(humanoid, chest, pose._chestRot)
bc.resetJointStateMultiDof(humanoid, neck, pose._neckRot)
bc.resetJointStateMultiDof(humanoid, rightHip, pose._rightHipRot)
bc.resetJointStateMultiDof(humanoid, rightKnee, pose._rightKneeRot)
bc.resetJointStateMultiDof(humanoid, rightAnkle, pose._rightAnkleRot)
bc.resetJointStateMultiDof(humanoid, rightShoulder, pose._rightShoulderRot)
bc.resetJointStateMultiDof(humanoid, rightElbow, pose._rightElbowRot)
bc.resetJointStateMultiDof(humanoid, leftHip, pose._leftHipRot)
bc.resetJointStateMultiDof(humanoid, leftKnee, pose._leftKneeRot)
bc.resetJointStateMultiDof(humanoid, leftAnkle, pose._leftAnkleRot)
bc.resetJointStateMultiDof(humanoid, leftShoulder, pose._leftShoulderRot)
bc.resetJointStateMultiDof(humanoid, leftElbow, pose._leftElbowRot)
bc.setJointMotorControlMultiDof(humanoid,
chest,
controlMode,
targetPosition=pose._chestRot,
positionGain=kp,
force=[200])
bc.setJointMotorControlMultiDof(humanoid,
neck,
controlMode,
targetPosition=pose._neckRot,
positionGain=kp,
force=[50])
bc.setJointMotorControlMultiDof(humanoid,
rightHip,
controlMode,
targetPosition=pose._rightHipRot,
positionGain=kp,
force=[200])
bc.setJointMotorControlMultiDof(humanoid,
rightKnee,
controlMode,
targetPosition=pose._rightKneeRot,
positionGain=kp,
force=[150])
bc.setJointMotorControlMultiDof(humanoid,
rightAnkle,
controlMode,
targetPosition=pose._rightAnkleRot,
positionGain=kp,
force=[90])
bc.setJointMotorControlMultiDof(humanoid,
rightShoulder,
controlMode,
targetPosition=pose._rightShoulderRot,
positionGain=kp,
force=[100])
bc.setJointMotorControlMultiDof(humanoid,
rightElbow,
controlMode,
targetPosition=pose._rightElbowRot,
positionGain=kp,
force=[60])
bc.setJointMotorControlMultiDof(humanoid,
leftHip,
controlMode,
targetPosition=pose._leftHipRot,
positionGain=kp,
force=[200])
bc.setJointMotorControlMultiDof(humanoid,
leftKnee,
controlMode,
targetPosition=pose._leftKneeRot,
positionGain=kp,
force=[150])
bc.setJointMotorControlMultiDof(humanoid,
leftAnkle,
controlMode,
targetPosition=pose._leftAnkleRot,
positionGain=kp,
force=[90])
bc.setJointMotorControlMultiDof(humanoid,
leftShoulder,
controlMode,
targetPosition=pose._leftShoulderRot,
positionGain=kp,
force=[100])
bc.setJointMotorControlMultiDof(humanoid,
leftElbow,
controlMode,
targetPosition=pose._leftElbowRot,
positionGain=kp,
force=[60])
#debug space
#if (False):
# for j in range (bc.getNumJoints(self._humanoid)):
# js = bc.getJointState(self._humanoid, j)
# bc.resetJointState(self._humanoidDebug, j,js[0])
# jsm = bc.getJointStateMultiDof(self._humanoid, j)
# if (len(jsm[0])>0):
# bc.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0])
def GetState(self):
stateVector = []
phase = self.GetPhase()
#print("phase=",phase)
stateVector.append(phase)
rootTransPos, rootTransOrn = self.BuildOriginTrans()
basePos, baseOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn,
basePos, [0, 0, 0, 1])
#print("!!!rootPosRel =",rootPosRel )
#print("rootTransPos=",rootTransPos)
#print("basePos=",basePos)
localPos, localOrn = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn,
basePos, baseOrn)
localPos = [
localPos[0] - rootPosRel[0], localPos[1] - rootPosRel[1], localPos[2] - rootPosRel[2]
]
#print("localPos=",localPos)
stateVector.append(rootPosRel[1])
self.pb2dmJoints = [0, 1, 2, 9, 10, 11, 3, 4, 5, 12, 13, 14, 6, 7, 8]
for pbJoint in range(self._pybullet_client.getNumJoints(self._humanoid)):
j = self.pb2dmJoints[pbJoint]
#print("joint order:",j)
ls = self._pybullet_client.getLinkState(self._humanoid, j, computeForwardKinematics=True)
linkPos = ls[0]
linkOrn = ls[1]
linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms(
rootTransPos, rootTransOrn, linkPos, linkOrn)
if (linkOrnLocal[3] < 0):
linkOrnLocal = [-linkOrnLocal[0], -linkOrnLocal[1], -linkOrnLocal[2], -linkOrnLocal[3]]
linkPosLocal = [
linkPosLocal[0] - rootPosRel[0], linkPosLocal[1] - rootPosRel[1],
linkPosLocal[2] - rootPosRel[2]
]
for l in linkPosLocal:
stateVector.append(l)
#re-order the quaternion, DeepMimic uses w,x,y,z
stateVector.append(linkOrnLocal[3])
stateVector.append(linkOrnLocal[0])
stateVector.append(linkOrnLocal[1])
stateVector.append(linkOrnLocal[2])
for pbJoint in range(self._pybullet_client.getNumJoints(self._humanoid)):
j = self.pb2dmJoints[pbJoint]
ls = self._pybullet_client.getLinkState(self._humanoid, j, computeLinkVelocity=True)
linkLinVel = ls[6]
linkAngVel = ls[7]
for l in linkLinVel:
stateVector.append(l)
for l in linkAngVel:
stateVector.append(l)
#print("stateVector len=",len(stateVector))
#for st in range (len(stateVector)):
# print("state[",st,"]=",stateVector[st])
return stateVector
def GetReward(self):
#from DeepMimic double cSceneImitate::CalcRewardImitate
pose_w = 0.5
vel_w = 0.05
end_eff_w = 0 #0.15
root_w = 0 #0.2
com_w = 0.1
total_w = pose_w + vel_w + end_eff_w + root_w + com_w
pose_w /= total_w
vel_w /= total_w
end_eff_w /= total_w
root_w /= total_w
com_w /= total_w
pose_scale = 2
vel_scale = 0.1
end_eff_scale = 40
root_scale = 5
com_scale = 10
err_scale = 1
reward = 0
pose_err = 0
vel_err = 0
end_eff_err = 0
root_err = 0
com_err = 0
heading_err = 0
#create a mimic reward, comparing the dynamics humanoid with a kinematic one
pose = self.InitializePoseFromMotionData()
#print("self._kinematicHumanoid=",self._kinematicHumanoid)
#print("kinematicHumanoid #joints=",self.kin_client.getNumJoints(self._kinematicHumanoid))
self.ApplyPose(pose, True, True, self._kinematicHumanoid, self.kin_client)
#const Eigen::VectorXd& pose0 = sim_char.GetPose();
#const Eigen::VectorXd& vel0 = sim_char.GetVel();
#const Eigen::VectorXd& pose1 = kin_char.GetPose();
#const Eigen::VectorXd& vel1 = kin_char.GetVel();
#tMatrix origin_trans = sim_char.BuildOriginTrans();
#tMatrix kin_origin_trans = kin_char.BuildOriginTrans();
#
#tVector com0_world = sim_char.CalcCOM();
#tVector com_vel0_world = sim_char.CalcCOMVel();
#tVector com1_world;
#tVector com_vel1_world;
#cRBDUtil::CalcCoM(joint_mat, body_defs, pose1, vel1, com1_world, com_vel1_world);
#
root_id = 0
#tVector root_pos0 = cKinTree::GetRootPos(joint_mat, pose0);
#tVector root_pos1 = cKinTree::GetRootPos(joint_mat, pose1);
#tQuaternion root_rot0 = cKinTree::GetRootRot(joint_mat, pose0);
#tQuaternion root_rot1 = cKinTree::GetRootRot(joint_mat, pose1);
#tVector root_vel0 = cKinTree::GetRootVel(joint_mat, vel0);
#tVector root_vel1 = cKinTree::GetRootVel(joint_mat, vel1);
#tVector root_ang_vel0 = cKinTree::GetRootAngVel(joint_mat, vel0);
#tVector root_ang_vel1 = cKinTree::GetRootAngVel(joint_mat, vel1);
mJointWeights = [
0.20833, 0.10416, 0.0625, 0.10416, 0.0625, 0.041666666666666671, 0.0625, 0.0416, 0.00,
0.10416, 0.0625, 0.0416, 0.0625, 0.0416, 0.0000
]
num_end_effs = 0
num_joints = 15
root_rot_w = mJointWeights[root_id]
#pose_err += root_rot_w * cKinTree::CalcRootRotErr(joint_mat, pose0, pose1)
#vel_err += root_rot_w * cKinTree::CalcRootAngVelErr(joint_mat, vel0, vel1)
for j in range(num_joints):
curr_pose_err = 0
curr_vel_err = 0
w = mJointWeights[j]
simJointInfo = self._pybullet_client.getJointStateMultiDof(self._humanoid, j)
#print("simJointInfo.pos=",simJointInfo[0])
#print("simJointInfo.vel=",simJointInfo[1])
kinJointInfo = self.kin_client.getJointStateMultiDof(self._kinematicHumanoid, j)
#print("kinJointInfo.pos=",kinJointInfo[0])
#print("kinJointInfo.vel=",kinJointInfo[1])
if (len(simJointInfo[0]) == 1):
angle = simJointInfo[0][0] - kinJointInfo[0][0]
curr_pose_err = angle * angle
velDiff = simJointInfo[1][0] - kinJointInfo[1][0]
curr_vel_err = velDiff * velDiff
if (len(simJointInfo[0]) == 4):
#print("quaternion diff")
diffQuat = self._pybullet_client.getDifferenceQuaternion(simJointInfo[0], kinJointInfo[0])
axis, angle = self._pybullet_client.getAxisAngleFromQuaternion(diffQuat)
curr_pose_err = angle * angle
diffVel = [
simJointInfo[1][0] - kinJointInfo[1][0], simJointInfo[1][1] - kinJointInfo[1][1],
simJointInfo[1][2] - kinJointInfo[1][2]
]
curr_vel_err = diffVel[0] * diffVel[0] + diffVel[1] * diffVel[1] + diffVel[2] * diffVel[2]
pose_err += w * curr_pose_err
vel_err += w * curr_vel_err
# bool is_end_eff = sim_char.IsEndEffector(j)
# if (is_end_eff)
# {
# tVector pos0 = sim_char.CalcJointPos(j)
# tVector pos1 = cKinTree::CalcJointWorldPos(joint_mat, pose1, j)
# double ground_h0 = mGround->SampleHeight(pos0)
# double ground_h1 = kin_char.GetOriginPos()[1]
#
# tVector pos_rel0 = pos0 - root_pos0
# tVector pos_rel1 = pos1 - root_pos1
# pos_rel0[1] = pos0[1] - ground_h0
# pos_rel1[1] = pos1[1] - ground_h1
#
# pos_rel0 = origin_trans * pos_rel0
# pos_rel1 = kin_origin_trans * pos_rel1
#
# curr_end_err = (pos_rel1 - pos_rel0).squaredNorm()
# end_eff_err += curr_end_err;
# ++num_end_effs;
# }
#}
#if (num_end_effs > 0):
# end_eff_err /= num_end_effs
#
#double root_ground_h0 = mGround->SampleHeight(sim_char.GetRootPos())
#double root_ground_h1 = kin_char.GetOriginPos()[1]
#root_pos0[1] -= root_ground_h0
#root_pos1[1] -= root_ground_h1
#root_pos_err = (root_pos0 - root_pos1).squaredNorm()
#
#root_rot_err = cMathUtil::QuatDiffTheta(root_rot0, root_rot1)
#root_rot_err *= root_rot_err
#root_vel_err = (root_vel1 - root_vel0).squaredNorm()
#root_ang_vel_err = (root_ang_vel1 - root_ang_vel0).squaredNorm()
#root_err = root_pos_err
# + 0.1 * root_rot_err
# + 0.01 * root_vel_err
# + 0.001 * root_ang_vel_err
#com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm()
#print("pose_err=",pose_err)
#print("vel_err=",vel_err)
pose_reward = math.exp(-err_scale * pose_scale * pose_err)
vel_reward = math.exp(-err_scale * vel_scale * vel_err)
end_eff_reward = math.exp(-err_scale * end_eff_scale * end_eff_err)
root_reward = math.exp(-err_scale * root_scale * root_err)
com_reward = math.exp(-err_scale * com_scale * com_err)
reward = pose_w * pose_reward + vel_w * vel_reward + end_eff_w * end_eff_reward + root_w * root_reward + com_w * com_reward
#print("reward = %f (pose_reward=%f, vel_reward=%f, end_eff_reward=%f, root_reward=%f, com_reward=%f)\n", reward,
# pose_reward,vel_reward,end_eff_reward, root_reward, com_reward);
return reward
def GetBasePosition(self):
pos, orn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid)
return pos
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_walk.txt" #humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt"
motion.Load(motionPath)
#print("numFrames = ", motion.NumFrames())
simTimeId = bc.addUserDebugParameter("simTime", 0, motion.NumFrames() - 1.1, 0)
y2zOrn = bc.getQuaternionFromEuler([-1.57, 0, 0])
bc.loadURDF("plane.urdf", [0, -0.04, 0], y2zOrn)
humanoid = Humanoid(bc, motion, [0, 0, 0]) #4000,0,5000])
simTime = 0
class ModularEnv(gym.Env):
"""Abstract modular environment"""
metadata = {'render.modes': ['human']}
def __init__(self, terrain=None):
# Create pybullet interfaces
self.client = BulletClient(connection_mode=pyb.DIRECT)
self._last_render = time.time()
# Setup information needed for simulation
self.dt = 1. / 240.
self.client.setAdditionalSearchPath(ASSET_PATH)
self.terrain_type = terrain
# Stored for user interactions
self.morphology = None
self.multi_id = None
self._joint_ids = []
self._joints = []
# Used for user interaction:
self._real_time = False
# Run setup
self.setup()
def load_terrain(self):
"""Helper method to load terrain for ground"""
if not self.terrain_type:
return None
elif type(self.terrain_type) is PNGTerrain:
shape = self.client.createCollisionShape(
shapeType=pyb.GEOM_HEIGHTFIELD,
meshScale=self.terrain_type.scale,
fileName=self.terrain_type.height_field)
assert shape >= 0, "Could not load PNGTerrain shape"
terrain = self.client.createMultiBody(0, shape)
if self.terrain_type.texture:
texture = self.client.loadTexture(self.terrain_type.texture)
assert texture >= 0, "Could not load PNGTerrain texture"
self.client.changeVisualShape(terrain,
-1,
textureUniqueId=texture)
if self.terrain_type.position:
self.client.resetBasePositionAndOrientation(
terrain, self.terrain_type.position, [0, 0, 0, 1])
elif type(self.terrain_type) is ArrayTerrain:
shape = self.client.createCollisionShape(
shapeType=pyb.GEOM_HEIGHTFIELD,
meshScale=self.terrain_type.scale,
heightfieldTextureScaling=(self.terrain_type.rows - 1) / 2.,
heightfieldData=self.terrain_type.field.flatten(),
numHeightfieldRows=self.terrain_type.rows,
numHeightfieldColumns=self.terrain_type.cols)
assert shape >= 0, "Could not create ArrayTerrain shape"
terrain = self.client.createMultiBody(0, shape)
self.client.resetBasePositionAndOrientation(
terrain, [0, 0, 0], [0, 0, 0, 1])
assert terrain >= 0, "Could not load terrain"
self.client.changeVisualShape(terrain, -1, rgbaColor=[1, 1, 1, 1])
return terrain
def setup(self):
"""Helper method to initialize default environment"""
# This is abstracted out from '__init__' because we need to do this
# first time 'render' is called
self.client.resetSimulation()
self.client.setGravity(0, 0, -9.81)
# Load ground plane for robots to walk on
self.plane_id = self.client.loadURDF('plane/plane.urdf',
useMaximalCoordinates=1)
assert self.plane_id >= 0, "Could not load 'plane.urdf'"
# Change dynamics parameters from:
# https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py#L45
self.client.changeDynamics(self.plane_id, -1, lateralFriction=0.9)
# self.client.setPhysicsEngineParameter(numSolverIterations=10)
# self.client.setPhysicsEngineParameter(minimumSolverIslandSize=100)
self.client.setPhysicsEngineParameter(contactERP=0)
# Extract time step for sleep during rendering
self.dt = self.client.getPhysicsEngineParameters()['fixedTimeStep']
def close(self):
self.client.disconnect()
def reset(self, morphology=None, max_size=None):
# Disable rendering during spawning
self.client.configureDebugVisualizer(pyb.COV_ENABLE_RENDERING, 0)
# Reset the environment by running all setup code again
self.setup()
# Reset internal state
self.morphology = None
self.multi_id = None
self._joint_ids = []
self._joints = []
# Check method arguments for simple logic errors
if morphology is None:
raise TypeError("Morphology cannot be 'None'!")
if max_size is not None and max_size < 1:
raise ValueError("'max_size' must be larger than 0")
# NOTE: Utilize deep copy here to avoid interfering with morphology,
# this means that if we want to do structural changes we can do so on
# our own private instance. Which is nice for maximum size etc.
self.morphology = copy.deepcopy(morphology.root)
# Mapping from module to PyBullet ID
spawned_ids = {}
# NOTE: We are using explicit queue handling here so that we can
# ignore children of overlapping modules
queue = deque([self.morphology.root])
while len(queue) > 0:
module = queue.popleft()
assert isinstance(module, Module3D), "{} does not inherit\
from 3D Module".format(type(module))
# Spawn module in world
m_id = module.spawn(self.client)
# Check if the module overlaps
aabb_min, aabb_max = self.client.getAABB(m_id)
# Check overlapping modules
overlap = self.client.getOverlappingObjects(aabb_min, aabb_max)
# NOTE: An object always collides with it self
overlapping_modules = False
if overlap:
overlapping_modules = any(
[u_id != m_id for u_id, _ in overlap])
# Check against plane
aabb_min, aabb_max = self.client.getAABB(self.plane_id)
plane_overlap = self.client.getOverlappingObjects(
aabb_min, aabb_max)
overlapping_plane = False
if plane_overlap:
overlapping_plane = any(
[u_id != self.plane_id for u_id, _ in plane_overlap])
# If overlap is detected de-spawn module and continue
if overlapping_modules or overlapping_plane:
# Remove from simulation
self.client.removeBody(m_id)
# Remove from our private copy
parent = module.parent
if parent:
del parent[module]
else:
raise RuntimeError(
"Trying to remove root module due to collision!")
continue
# Add children to queue for processing
queue.extend(module.children)
# Add ID to spawned IDs so that we can remove them later
spawned_ids[module] = m_id
# Add joint if present
if module.joint:
self._joints.append(module.joint)
# Check size constraints
if max_size is not None and len(spawned_ids) >= max_size:
# If we are above max desired spawn size drain queue and remove
for module in queue:
parent = module.parent
if parent:
del parent[module]
else:
raise RuntimeError("Trying to prune root link!")
break
# Create multi body builder and instantiate with morphological tree and
# module IDs
builder = MultiBodyBuilder(self.morphology,
spawned_ids,
self.client,
flags=pyb.URDF_USE_SELF_COLLISION)
# Before spawning multi body we remove all modules so far
# NOTE: We must do that here after all modules have been spawned to not
# interfere with collision detection
for m_id in spawned_ids.values():
self.client.removeBody(m_id)
# Instantiate the builder creating a multi body from the morphology
multi_id = builder.build()
assert multi_id >= 0, "Could not spawn multibody!"
self.multi_id = multi_id
for jid in range(self.client.getNumJoints(self.multi_id)):
j_info = self.client.getJointInfo(self.multi_id, jid)
if j_info[2] != pyb.JOINT_FIXED:
self._joint_ids.append(jid)
self.observation_space = gym.spaces.Box(-100.,
100.,
shape=(3 *
len(self._joints), ),
dtype=np.float64)
lows = np.repeat(-1.57, len(self._joints))
self.action_space = gym.spaces.Box(lows, -1. * lows, dtype=np.float64)
# Load additional terrain if requested
# NOTE: We load terrain after collision detection to avoid collisions
# with terrain itself
if self.load_terrain() is not None:
# If a different terrain was loaded we remove the plane
self.client.removeBody(self.plane_id)
self.plane_id = None
# Enable rendering here
self.client.configureDebugVisualizer(pyb.COV_ENABLE_RENDERING, 1)
return self.observation()
def act(self, action):
"""Helper function to apply actions to robot"""
assert action.shape[0] == len(self._joint_ids), 'Action not equal to\
number of joints'
for j_id, cfg, act in zip(self._joint_ids, self._joints, action):
# Create a local copy so we can delete from it
l_cfg = cfg.copy()
# If joint should be limited we check that here
if 'limit' in l_cfg:
act = max(l_cfg['limit'][0], min(l_cfg['limit'][1], act))
del l_cfg['limit']
# Extract type of 'target' for joint
l_cfg[l_cfg['target']] = act
del l_cfg['target']
self.client.setJointMotorControl2(self.multi_id, j_id, **l_cfg)
def step(self, action):
self.act(np.asarray(action))
self.client.stepSimulation()
return self.observation(), self.reward(), False, {}
def observation(self):
"""Get observation from environment
The default observation is `numpy.concatenate([positions, velocities,
torques])` for all movable joints."""
# If the body does not have any movable joints:
if not self._joint_ids:
return np.array([])
positions = []
velocities = []
torques = []
states = self.client.getJointStates(self.multi_id, self._joint_ids)
for state in states:
positions.append(state[0])
velocities.append(state[1])
torques.append(state[3])
return np.concatenate([positions, velocities, torques])
def reward(self):
"""Estimate current reward"""
# Get state of root link
pos, _ = self.client.getBasePositionAndOrientation(self.multi_id)
# Extract position
position = np.array(pos)
# We are only interested in distance in (X, Y)
position[-1] = 0.0
# Return the root's distance from World center
return np.linalg.norm(position)
def render(self, mode="human"):
info = self.client.getConnectionInfo()
if info['connectionMethod'] != pyb.GUI and mode == 'human':
# Close current simulation and start new with GUI
self.close()
self.client = BulletClient(connection_mode=pyb.GUI)
# Disable rendering during spawning
self.client.configureDebugVisualizer(pyb.COV_ENABLE_RENDERING, 0)
# Reset if morphology is set
if self.morphology is not None:
self.reset(self.morphology)
else:
self.setup()
# Configure viewport
self.client.configureDebugVisualizer(pyb.COV_ENABLE_GUI, 0)
self.client.configureDebugVisualizer(
pyb.COV_ENABLE_PLANAR_REFLECTION, 1)
self.client.resetDebugVisualizerCamera(0.5, 50.0, -35.0, (0, 0, 0))
# Setup timing for correct sleeping when rendering
self._last_render = time.time()
elif info['connectionMethod'] == pyb.GUI:
# Handle interaction with simulation
self.handle_interaction()
# Calculate time to sleep
now = time.time()
diff = now - self._last_render
to_sleep = self.dt - diff
if to_sleep > 0:
time.sleep(to_sleep)
self._last_render = now
else:
raise RuntimeError(
"Unknown pybullet mode ({}) or render mode ({})".format(
info['connectionMethod'], mode))
def handle_interaction(self):
"""Handle user interaction with simulation
This function is called in the 'render' call and is only called if the
GUI is visible"""
keys = self.client.getKeyboardEvents()
# If 'n' is pressed then we want to step the simulation
next_key = ord('n')
if next_key in keys and keys[next_key] & pyb.KEY_WAS_TRIGGERED:
self.client.stepSimulation()
# If space is pressed we start/stop real-time simulation
space = ord(' ')
if space in keys and keys[space] & pyb.KEY_WAS_TRIGGERED:
self._real_time = not self._real_time
self.client.setRealTimeSimulation(self._real_time)
# if 'r' is pressed we restart simulation
r = ord('r')
if r in keys and keys[r] & pyb.KEY_WAS_TRIGGERED:
real_time = self._real_time
self.client.setRealTimeSimulation(False)
self.reset(self.morphology)
self.client.setRealTimeSimulation(real_time)
def run():
"""Load URDF in PyBullet, move arms from initial to target state.
"""
# Start simulation and load URDF
client = BulletClient(pb.GUI)
client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
with prl_ur5_robot_urdf() as filename:
body_id = client.loadURDF(filename, useFixedBase=1)
client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
# Map joint names to indices
joint_map = {}
for i in range(client.getNumJoints(body_id)):
info = client.getJointInfo(body_id, i)
name = info[1].decode('utf-8', 'strict')
joint_map[name] = i
# Show joints map
print('Robot joints:')
for name, i in joint_map.items():
print('{}: {}'.format(i, name))
# Reset joints at initial state
init_state = {
'left_elbow_joint': -1.57,
'left_shoulder_lift_joint': -1.57,
'left_shoulder_pan_joint': -1.57,
'left_wrist_1_joint': 0,
'left_wrist_2_joint': 1.57,
'left_wrist_3_joint': -0.785,
'right_elbow_joint': 1.57,
'right_shoulder_lift_joint': -1.57,
'right_shoulder_pan_joint': 1.57,
'right_wrist_1_joint': -3.14,
'right_wrist_2_joint': -1.57,
'right_wrist_3_joint': 0.785,
}
for name, value in init_state.items():
client.resetJointState(body_id, joint_map[name], value)
# Apply target state
target_state = {
'left_elbow_joint': -1.57,
'left_shoulder_lift_joint': -1.57,
'left_shoulder_pan_joint': -1.57,
'left_wrist_1_joint': -1.57,
'left_wrist_2_joint': 2.36,
'left_wrist_3_joint': -1.57,
'right_elbow_joint': 1.57,
'right_shoulder_lift_joint': -1.57,
'right_shoulder_pan_joint': 1.57,
'right_wrist_1_joint': -1.57,
'right_wrist_2_joint': -2.36,
'right_wrist_3_joint': 1.57,
}
client.setJointMotorControlArray(
bodyUniqueId=body_id,
jointIndices=[joint_map[name] for name in target_state],
controlMode=pb.POSITION_CONTROL,
targetPositions=[value for value in target_state.values()],
forces=[10]*len(target_state))
# Simulate until the user to close the window
while client.isConnected():
client.stepSimulation()
time.sleep(0.1)
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_walk.txt"#humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt"
motion.Load(motionPath)
#print("numFrames = ", motion.NumFrames())
simTimeId= bc.addUserDebugParameter("simTime",0,motion.NumFrames()-1.1,0)
y2zOrn = bc.getQuaternionFromEuler([-1.57,0,0])
bc.loadURDF("plane.urdf",[0,-0.04,0], y2zOrn)
humanoid = Humanoid(bc, motion,[0,0,0])#4000,0,5000])
simTime = 0
class HandEnv(gym.Env):
"""The base Hand Environment class."""
# The environment can have both or a single hand
HAND_RIGHT = 1
HAND_LEFT = 2
HAND_BOTH = HAND_LEFT + HAND_RIGHT
# The environment can be controlled in joint or MANO-pose spaces
MODE_JOINT = 1
MODE_MANO = 2
MODE_LIST = (MODE_JOINT, MODE_MANO)
# Shape betas magnitude
SHAPE_BETAS_MAGNITUDE = 0.1
metadata = {
'render.modes': ['rgb_array'],
'video.frames_per_second': 60.0,
'video.output_frames_per_second': 30.0,
'step_time': 1.0 / 60.0}
def __init__(self,
hand_side=HAND_RIGHT,
control_mode=MODE_JOINT,
hand_model_cls=HandModel20,
randomize_hand_shape=False):
"""Constructor of a HandEnv.
Keyword Arguments:
hand_side {int} -- hand side: left, right or both (default: {HAND_BOTH})
control_mode {int} -- control modalities (default: {MODE_JOINT})
hand_model_cls {type} -- hand kinematic model (default: {HandModel20})
randomize_hand_shape {bool} -- randomize hand shape at reset (default: {True})
"""
assert hand_side & self.HAND_BOTH, f'Wrong hand side flag: {hand_side}'
assert control_mode in self.MODE_LIST, f'Wrong control mode flag: {control_mode}'
self._hand_side = hand_side
self._control_mode = control_mode
self._randomize_hand_shape = randomize_hand_shape
self._show_window = False
self._random = None
self._client = None
self._hand_bodies = None
self._camera_shape = (320, 240)
self._camera_view = None
self._camera_proj = None
self._hand_models = []
if self.HAND_LEFT & hand_side:
self._hand_models.append(hand_model_cls(left_hand=True))
if self.HAND_RIGHT & hand_side:
self._hand_models.append(hand_model_cls(left_hand=False))
# if control in joint space
if self.MODE_JOINT == control_mode:
joint_low, joint_high = map(np.float32, self._hand_models[0].dofs_limits)
self.action_space = gym.spaces.Tuple([
gym.spaces.Tuple((
gym.spaces.Box(-5.0, 5.0, shape=(3,)), # base position x,y,z
gym.spaces.Box(-1.0, 1.0, shape=(4,)), # base quaternion x,y,z,w
gym.spaces.Box(joint_low, joint_high), # joint angles
)) for _ in self._hand_models])
self.observation_space = gym.spaces.Tuple([
gym.spaces.Tuple((
gym.spaces.Box(-5.0, 5.0, shape=(3,)), # base position x,y,z
gym.spaces.Box(-1.0, 1.0, shape=(4,)), # base quaternion x,y,z,w
gym.spaces.Box(-10.0, 10.0, shape=(6,)), # base constraint forces
gym.spaces.Box(joint_low, joint_high), # joint angles
gym.spaces.Box(-3.0, 3.0, shape=(len(joint_low),)), # joint velocities
gym.spaces.Box(-1.0, 1.0, shape=(len(joint_low),)), # joint torques
)) for _ in self._hand_models])
# if control in MANO space
elif self.MODE_MANO == control_mode:
self.action_space = gym.spaces.Tuple([
gym.spaces.Tuple((
gym.spaces.Box(-5.0, 5.0, shape=(3,)), # trans
gym.spaces.Box(-np.pi, np.pi, shape=(16, 3)), # pose
)) for _ in self._hand_models])
self.observation_space = gym.spaces.Tuple([
gym.spaces.Tuple((
gym.spaces.Box(-5.0, 5.0, shape=(3,)), # trans
gym.spaces.Box(-np.pi, np.pi, shape=(16, 3)), # pose
)) for _ in self._hand_models])
def show_window(self, show=True):
"""Show GUI window or not.
Keyword Arguments:
show {bool} -- show flag (default: {True})
"""
self._show_window = show
def reset(self, initial_hands_state=None, **_kwargs):
"""Resets the environment to an initial state and returns an initial observation.
Keyword Arguments:
initial_hands_state {int} -- override the initial hands state (default: {None})
Returns:
observation (object): the initial observation.
"""
if self._client is None:
self._client = BulletClient(pb.GUI if self._show_window else pb.DIRECT)
self._client.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0)
self._setup_camera()
self._client.resetSimulation()
self._client.setAdditionalSearchPath(pd.getDataPath())
self._client.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
self._client.setGravity(0, 0, -10)
self._client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0)
# randomize the handshape if needed
betas = None
if self._randomize_hand_shape:
betas = self._random.rand(10) * self.SHAPE_BETAS_MAGNITUDE
# spawn the hands
self._hand_bodies = []
for i, model in enumerate(self._hand_models):
hand_body = HandBody(self._client, model, shape_betas=betas)
if initial_hands_state is not None:
pos, orn, angles = initial_hands_state[i]
else:
pos_y, orn_z = (0.10, 0.0) if model.is_left_hand else (-0.10, -np.pi)
pos, orn = (0.0, pos_y, 0.25), pb.getQuaternionFromEuler((np.pi/2, 0.0, orn_z))
angles = np.zeros(len(hand_body.joint_indices))
hand_body.reset(pos, orn, angles)
hand_body.set_target(pos, orn, angles)
self._hand_bodies.append(hand_body)
self._client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1)
return self._get_observation()
def step(self, action):
"""Run one timestep of the environment's dynamics.
Args:
action (object): an action provided by the agent
Returns:
observation (object): agent's observation of the current environment
reward (float) : amount of reward returned after previous action
done (bool): whether the episode has ended
info (dict): contains auxiliary diagnostic information
"""
self._take_action(action)
num_steps = int(self.metadata.get('step_time') * 240.0)
for _ in range(num_steps):
self._client.stepSimulation()
observation = self._get_observation()
reward = self._get_reward(observation)
done = self._is_done(observation)
info = {}
return observation, reward, done, info
def render(self, mode='human'):
"""Renders the environment.
Args:
mode (str): the mode to render with
"""
if mode == 'rgb_array':
data = pb.getCameraImage(*self._camera_shape, self._camera_view, self._camera_proj)
rgba = data[2]
return rgba[:, :, :3]
return super().render(mode=mode)
def close(self):
"""Perform necessary cleanup.
Environments will automatically close() themselves when
garbage collected or when the program exits.
"""
if self._client.isConnected():
self._client.disconnect()
def seed(self, seed=None):
"""Sets the seed for this env's random number generator(s).
Args:
seed (int): provided number generators seed
"""
self._random = RandomState(seed)
return [seed]
def _take_action(self, action):
"""Compute observation of the current environment.
Args:
action (object): an action provided by the agent
"""
if self.MODE_JOINT == self._control_mode:
for hand, (pos, orn, angles) in zip(self._hand_bodies, action):
hand.set_target(pos, orn, angles)
elif self.MODE_MANO == self._control_mode:
for hand, (trans, pose) in zip(self._hand_bodies, action):
hand.set_target_from_mano(trans, pose)
def _get_observation(self):
"""Compute observation of the current environment.
Returns:
observation (object): agent's observation of the current environment
"""
if self.MODE_JOINT == self._control_mode:
return [hand.get_state() for hand in self._hand_bodies]
if self.MODE_MANO == self._control_mode:
return [hand.get_mano_state() for hand in self._hand_bodies]
return None
def _get_reward(self, observation):
"""Compute reward at the current environment state.
Returns:
observation (object): agent's observation of the current environment
"""
# pylint: disable=no-self-use, unused-argument
return 0.0
def _is_done(self, observation):
"""Check if the current environment state is final.
Returns:
observation (object): agent's observation of the current environment
"""
# pylint: disable=no-self-use, unused-argument
return False
def _setup_camera(self):
"""Setup virtual camera."""
self._client.resetDebugVisualizerCamera(
cameraDistance=0.5,
cameraYaw=-45.0,
cameraPitch=-40.0,
cameraTargetPosition=[0, 0, 0.1])
self._camera_view = self._client.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=[0.0, 0.0, 0.1],
distance=0.5,
yaw=-45.0,
pitch=-40.0,
roll=0.0,
upAxisIndex=2)
self._camera_proj = self._client.computeProjectionMatrixFOV(
fov=60.0,
aspect=self._camera_shape[0] / self._camera_shape[1],
nearVal=0.1,
farVal=10.0)
class LocomotionGymEnv(gym.Env):
def __init__(self,
gym_config: SimulationParameters,
robot_class=None,
robot_params: RobotSimParams = None,
task: BaseTask = None):
self.seed()
self._gym_config = gym_config
if robot_class is None:
raise ValueError('robot_class cannot be None.')
self._robot_class = robot_class
# A dictionary containing the objects in the world other than the robot.
self._world_dict = {}
self._robot_params = robot_params
self._task = task
self._obs_sensor = None
self._time_step = self._gym_config.time_step
self._num_action_repeat = self._gym_config.num_action_repeat
# self._num_bullet_solver_iterations = 12
self._is_render = self._gym_config.enable_rendering and (
not self._gym_config.egl_rendering)
if self._is_render:
self._pybullet_client = BulletClient(connection_mode=p.GUI)
self._pybullet_client.configureDebugVisualizer(
p.COV_ENABLE_GUI, gym_config.enable_rendering_gui)
else:
self._pybullet_client = BulletClient(connection_mode=p.DIRECT)
self._pybullet_client.setAdditionalSearchPath(
pybullet_data.getDataPath())
self._last_frame_time = 0.0
# set egl acceleration
if self._gym_config.egl_rendering:
egl = pkgutil.get_loader('eglRenderer')
self._eglPlugin = self._pybullet_client.loadPlugin(
egl.get_filename(), "_eglRendererPlugin")
self._build_action_space()
self._build_observation_space()
# Set the default render options.
self._camera_dist = self._gym_config.camera_distance
self._camera_yaw = self._gym_config.camera_yaw
self._camera_pitch = self._gym_config.camera_pitch
self._render_width = self._gym_config.render_width
self._render_height = self._gym_config.render_height
self._hard_reset = True
self._num_env_act = 0
self.set_gui_sliders()
self._contact_fall = ContactDetection(self)
self._reset_time = self._gym_config.reset_time
self.reset(start_motor_angles=None, reset_duration=self._reset_time)
if self._task is not None:
self._task.reset(self)
self._hard_reset = self._gym_config.enable_hard_reset
# global_values.global_userDebugParams = UserDebugParams(self._pybullet_client)
# global_values.global_userDebugParams.setPbClient(self._pybullet_client)
# set_gui_sliders(self._pybullet_client)
def _build_action_space(self):
motor_mode = self._robot_params.motor_control_mode
action_upper_bound = []
action_lower_bound = []
if (motor_mode == MotorControlMode.POSITION) or (
motor_mode == MotorControlMode.HYBRID_COMPUTED_POS):
action_lower_bound.extend(self._robot_params.joint_angle_MinMax[0])
action_upper_bound.extend(self._robot_params.joint_angle_MinMax[1])
# action_config = self._robot_params.JOINT_ANGLE_LIMIT
# for action in action_config:
# action_upper_bound.append(action.upper_bound)
# action_lower_bound.append(action.lower_bound)
elif motor_mode == MotorControlMode.TORQUE:
action_lower_bound.extend(
self._robot_params.joint_torque_MinMax[0])
action_upper_bound.extend(
self._robot_params.joint_torque_MinMax[1])
# action_config = self._robot_params.JOINT_TORQUE_LIMIT
# for action in action_config:
# action_upper_bound.append(action.upper_bound)
# action_lower_bound.append(action.lower_bound)
elif motor_mode == MotorControlMode.HYBRID_COMPUTED_POS_VEL:
action_lower_bound.extend(self._robot_params.joint_angle_MinMax[0])
action_lower_bound.extend(
self._robot_params.joint_velocity_MinMax[0])
action_upper_bound.extend(self._robot_params.joint_angle_MinMax[1])
action_upper_bound.extend(
self._robot_params.joint_velocity_MinMax[1])
# for action_config in [self._robot_params.JOINT_ANGLE_LIMIT, self._robot_params.JOINT_VELOCITY_LIMIT]:
# for action in action_config:
# action_upper_bound.append(action.upper_bound)
# action_lower_bound.append(action.lower_bound)
elif motor_mode == MotorControlMode.HYBRID_COMPUTED_POS_SINGLE:
action_lower_bound.extend(
self._robot_params.joint_angle_MinMax[0][:3])
action_upper_bound.extend(
self._robot_params.joint_angle_MinMax[1][:3])
elif motor_mode == MotorControlMode.HYBRID_COMPUTED_POS_TROT:
action_lower_bound.extend(
self._robot_params.joint_angle_MinMax[0][:6])
action_upper_bound.extend(
self._robot_params.joint_angle_MinMax[1][:6])
self.action_space = spaces.Box(np.array(action_lower_bound),
np.array(action_upper_bound),
dtype=np.float32)
def _build_observation_space(self):
if self._task is None:
return
else:
self._obs_sensor = SensorWrapper(
self._task.get_observation_sensors())
self.observation_space = spaces.Box(self._obs_sensor.get_lower_bound(),
self._obs_sensor.get_upper_bound(),
dtype=np.float32)
def seed(self, seed=None):
self.np_random, self.np_random_seed = seeding.np_random(seed)
return [self.np_random_seed]
def reset(self,
start_motor_angles=None,
reset_duration=0.002,
reset_visualization_camera=True,
force_hard_reset=False):
if self._is_render:
self._pybullet_client.configureDebugVisualizer(
p.COV_ENABLE_RENDERING, 0)
# Clear the simulation world and rebuild the robot interface.
if force_hard_reset or self._hard_reset:
self._pybullet_client.resetSimulation()
# self._pybullet_client.setPhysicsEngineParameter(numSolverIterations=self._num_bullet_solver_iterations)
self._pybullet_client.setTimeStep(self._time_step)
self._pybullet_client.setGravity(0, 0, -9.8)
# Rebuild the world.
self._world_dict = {
"ground": self._pybullet_client.loadURDF("plane_implicit.urdf")
}
# Rebuild the robot
self._robot = self._robot_class(
self._pybullet_client,
self._robot_params,
time_step=self._time_step,
num_action_repeat=self._num_action_repeat)
if self._obs_sensor is not None:
self._obs_sensor.set_robot(self._robot)
# Reset the pose of the robot.
# self._robot.Reset(reload_urdf=False, default_motor_angles=start_motor_angles, reset_time=reset_duration)
if self._is_render and reset_visualization_camera:
self._pybullet_client.resetDebugVisualizerCamera(
self._camera_dist, self._camera_yaw, self._camera_pitch,
[0, 0, 0])
if self._is_render:
self._pybullet_client.configureDebugVisualizer(
p.COV_ENABLE_RENDERING, 1)
self._robot.Reset(reload_urdf=False,
default_motor_angles=start_motor_angles,
reset_time=reset_duration)
if self._obs_sensor is not None:
self._obs_sensor.on_reset()
self._contact_fall.reset()
self._num_env_act = 0
return self._get_observation()
def step(self, action):
time_spent = time.time() - self._last_frame_time
self._last_frame_time = time.time()
# if self._num_env_act % 1 == 0:
# print(f"time_spent: {time_spent * 1000} ms")
if self._is_render:
# Sleep, otherwise the computation takes less time than real time,
# which will make the visualization like a fast-forward video.
time_to_sleep = self._time_step * self._num_action_repeat - time_spent
if time_to_sleep > 0:
time.sleep(time_to_sleep)
# action = action * np.asarray(self.action_space.high)
self._robot.Step(action, None)
if self._obs_sensor is not None:
self._obs_sensor.on_step()
self._num_env_act += 1
return self._get_observation(), self._get_reward(), self.done, {}
def render(self, mode='rgb_array'):
if mode != 'rgb_array':
raise ValueError('Unsupported render mode:{}'.format(mode))
base_pos = self._robot.GetBasePosition()
view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll(
cameraTargetPosition=base_pos,
distance=self._camera_dist,
yaw=self._camera_yaw,
pitch=self._camera_pitch,
roll=0,
upAxisIndex=2)
proj_matrix = self._pybullet_client.computeProjectionMatrixFOV(
fov=60,
aspect=float(self._render_width) / self._render_height,
nearVal=0.1,
farVal=100.0)
(_, _, px, _, _) = self._pybullet_client.getCameraImage(
width=self._render_width,
height=self._render_height,
renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL,
viewMatrix=view_matrix,
projectionMatrix=proj_matrix)
rgb_array = np.array(px)
rgb_array = rgb_array[:, :, :3]
return rgb_array
def close(self):
if hasattr(self, '_robot') and self._robot:
self._robot.Terminate()
def getRobotID(self):
return self._robot.getRobotID()
@property
def robot(self):
return self._robot
@property
def ground(self):
return self._world_dict["ground"]
def getClient(self):
return self._pybullet_client
@property
def env_step_counter(self):
return self._num_env_act
@property
def baseHeight(self):
return self._robot.ReadBaseHeight()
def set_gui_sliders(self):
set_gui_sliders(self._pybullet_client)
def _get_observation(self):
if self._obs_sensor is not None:
return self._obs_sensor.get_observation()
else:
return None
def _get_reward(self):
if self._task is not None:
return self._task.reward()
else:
return None
@property
def done(self) -> bool:
# if self._obs_sensor is not None:
# if self._obs_sensor.on_terminate():
# return True
if self._contact_fall.is_contact_fall():
return True
if self._task is not None:
return self._task.done()
else:
return False
class PyBullet(Environment):
"""
Class to create a Mushroom environment using the PyBullet simulator.
"""
def __init__(self,
files,
actuation_spec,
observation_spec,
gamma,
horizon,
timestep=1 / 240,
n_intermediate_steps=1,
enforce_joint_velocity_limits=False,
debug_gui=False,
**viewer_params):
"""
Constructor.
Args:
files (dict): dictionary of the URDF/MJCF/SDF files to load (key) and parameters dictionary (value);
actuation_spec (list): A list of tuples specifying the names of the
joints which should be controllable by the agent and tehir control mode.
Can be left empty when all actuators should be used in position control;
observation_spec (list): A list containing the names of data that
should be made available to the agent as an observation and
their type (ObservationType). An entry in the list is given by:
(name, type);
gamma (float): The discounting factor of the environment;
horizon (int): The maximum horizon for the environment;
timestep (float, 0.00416666666): The timestep used by the PyBullet
simulator;
n_intermediate_steps (int): The number of steps between every action
taken by the agent. Allows the user to modify, control and
access intermediate states;
enforce_joint_velocity_limits (bool, False): flag to enforce the velocity limits;
debug_gui (bool, False): flag to activate the default pybullet visualizer, that can be used for debug
purposes;
**viewer_params: other parameters to be passed to the viewer.
See PyBulletViewer documentation for the available options.
"""
assert len(observation_spec) > 0
assert len(actuation_spec) > 0
# Store simulation parameters
self._timestep = timestep
self._n_intermediate_steps = n_intermediate_steps
# Create the simulation and viewer
if debug_gui:
self._client = BulletClient(connection_mode=pybullet.GUI)
self._client.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0)
else:
self._client = BulletClient(connection_mode=pybullet.DIRECT)
self._client.setTimeStep(self._timestep)
self._client.setGravity(0, 0, -9.81)
self._client.setAdditionalSearchPath(pybullet_data.getDataPath())
self._viewer = PyBulletViewer(self._client, self.dt, **viewer_params)
self._state = None
# Load model and create access maps
self._model_map = dict()
self._load_all_models(files, enforce_joint_velocity_limits)
# Build utils
self._indexer = IndexMap(self._client, self._model_map, actuation_spec,
observation_spec)
self.joints = JointsHelper(self._client, self._indexer,
observation_spec)
# Finally, we create the MDP information and call the constructor of the parent class
action_space = Box(*self._indexer.action_limits)
observation_space = Box(*self._indexer.observation_limits)
mdp_info = MDPInfo(observation_space, action_space, gamma, horizon)
# Let the child class modify the mdp_info data structure
mdp_info = self._modify_mdp_info(mdp_info)
# Provide the structure to the superclass
super().__init__(mdp_info)
# Save initial state of the MDP
self._initial_state = self._client.saveState()
def seed(self, seed):
np.random.seed(seed)
def reset(self, state=None):
self._client.restoreState(self._initial_state)
self.setup(state)
self._state = self._indexer.create_sim_state()
observation = self._create_observation(self._state)
return observation
def render(self):
self._viewer.display()
def stop(self):
self._viewer.close()
def step(self, action):
curr_state = self._state.copy()
action = self._preprocess_action(action)
self._step_init(curr_state, action)
for i in range(self._n_intermediate_steps):
ctrl_action = self._compute_action(curr_state, action)
self._indexer.apply_control(ctrl_action)
self._simulation_pre_step()
self._client.stepSimulation()
self._simulation_post_step()
curr_state = self._indexer.create_sim_state()
self._step_finalize()
absorbing = self.is_absorbing(curr_state)
reward = self.reward(self._state, action, curr_state, absorbing)
observation = self._create_observation(curr_state)
self._state = curr_state
return observation, reward, absorbing, {}
def get_sim_state_index(self, name, obs_type):
return self._indexer.get_index(name, obs_type)
def get_sim_state(self, obs, name, obs_type):
"""
Returns a specific observation value
Args:
obs (np.ndarray): the observation vector;
name (str): the name of the object to consider;
obs_type (PyBulletObservationType): the type of observation to be used.
Returns:
The required elements of the input state vector.
"""
indices = self.get_sim_state_index(name, obs_type)
return obs[indices]
def _modify_mdp_info(self, mdp_info):
"""
This method can be overridden to modify the automatically generated MDPInfo data structure.
By default, returns the given mdp_info structure unchanged.
Args:
mdp_info (MDPInfo): the MDPInfo structure automatically computed by the environment.
Returns:
The modified MDPInfo data structure.
"""
return mdp_info
def _create_observation(self, state):
"""
This method can be overridden to ctreate an observation vector from the simulator state vector.
By default, returns the simulator state vector unchanged.
Args:
state (np.ndarray): the simulator state vector.
Returns:
The environment observation.
"""
return state
def _load_model(self, file_name, kwargs):
if file_name.endswith('.urdf'):
model_id = self._client.loadURDF(file_name, **kwargs)
elif file_name.endswith('.sdf'):
model_id = self._client.loadSDF(file_name, **kwargs)[0]
else:
model_id = self._client.loadMJCF(file_name, **kwargs)[0]
for j in range(self._client.getNumJoints(model_id)):
self._client.setJointMotorControl2(model_id,
j,
pybullet.POSITION_CONTROL,
force=0)
return model_id
def _load_all_models(self, files, enforce_joint_velocity_limits):
for file_name, kwargs in files.items():
model_id = self._load_model(file_name, kwargs)
model_name = self._client.getBodyInfo(model_id)[1].decode('UTF-8')
self._model_map[model_name] = model_id
self._model_map.update(self._custom_load_models())
# Enforce velocity limits on every joint
if enforce_joint_velocity_limits:
for model_id in self._model_map.values():
for joint_id in range(self._client.getNumJoints(model_id)):
joint_data = self._client.getJointInfo(model_id, joint_id)
self._client.changeDynamics(
model_id, joint_id, maxJointVelocity=joint_data[11])
def _preprocess_action(self, action):
"""
Compute a transformation of the action provided to the
environment.
Args:
action (np.ndarray): numpy array with the actions
provided to the environment.
Returns:
The action to be used for the current step
"""
return action
def _step_init(self, state, action):
"""
Allows information to be initialized at the start of a step.
"""
pass
def _compute_action(self, state, action):
"""
Compute a transformation of the action at every intermediate step.
Useful to add control signals simulated directly in python.
Args:
state (np.ndarray): numpy array with the current state of teh simulation;
action (np.ndarray): numpy array with the actions, provided at every step.
Returns:
The action to be set in the actual pybullet simulation.
"""
return action
def _simulation_pre_step(self):
"""
Allows information to be accesed and changed at every intermediate step
before taking a step in the pybullet simulation.
Can be usefull to apply an external force/torque to the specified bodies.
"""
pass
def _simulation_post_step(self):
"""
Allows information to be accesed at every intermediate step
after taking a step in the pybullet simulation.
Can be usefull to average forces over all intermediate steps.
"""
pass
def _step_finalize(self):
"""
Allows information to be accesed at the end of a step.
"""
pass
def _custom_load_models(self):
"""
Allows to custom load a set of objects in the simulation
Returns:
A dictionary with the names and the ids of the loaded objects
"""
return list()
def reward(self, state, action, next_state, absorbing):
"""
Compute the reward based on the given transition.
Args:
state (np.array): the current state of the system;
action (np.array): the action that is applied in the current state;
next_state (np.array): the state reached after applying the given action;
absorbing (bool): whether next_state is an absorbing state or not.
Returns:
The reward as a floating point scalar value.
"""
raise NotImplementedError
def is_absorbing(self, state):
"""
Check whether the given state is an absorbing state or not.
Args:
state (np.array): the state of the system.
Returns:
A boolean flag indicating whether this state is absorbing or not.
"""
raise NotImplementedError
def setup(self, state):
"""
A function that allows to execute setup code after an environment
reset.
Args:
state (np.ndarray): the state to be restored. If the state should be
chosen by the environment, state is None. Environments can ignore this
value if the initial state cannot be set programmatically.
"""
pass
@property
def client(self):
return self._client
@property
def dt(self):
return self._timestep * self._n_intermediate_steps