def load_box(pybullet_client: bullet_client.BulletClient,
half_extents: Sequence[float] = (1, 1, 1),
position: Sequence[float] = (0, 0, 0),
orientation: Sequence[float] = (0, 0, 0, 1),
rgba_color: Sequence[float] = (0.3, 0.3, 0.3, 1),
mass: float = 0) -> int:
"""Loads a visible and tangible box.
Args:
half_extents: Half the dimension of the box in meters in each direction.
position: Global coordinates of the center of the box.
orientation: As a quaternion.
rgba_color: The color and transparency of the box, each in the range
[0,1]. Defaults to opaque gray.
mass: Mass in kg. Mass 0 fixes the box in place.
Returns:
Unique integer referring to the loaded box.
"""
col_box_id = pybullet_client.createCollisionShape(pybullet.GEOM_BOX,
halfExtents=half_extents)
visual_box_id = pybullet_client.createVisualShape(pybullet.GEOM_BOX,
halfExtents=half_extents,
rgbaColor=rgba_color)
return pybullet_client.createMultiBody(baseMass=mass,
baseCollisionShapeIndex=col_box_id,
baseVisualShapeIndex=visual_box_id,
basePosition=position,
baseOrientation=orientation)
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 __init__(self):
BaseRenderer.__init__(self) # <- important
ShowBase.__init__(self)
client = BulletClient(pb.DIRECT)
client.setAdditionalSearchPath(pybullet_data.getDataPath())
self.client = client
# bind external renderer
plugin = RenderingPlugin(client, self)
# setup scene
self.nodes = {}
self.camLens.setNearFar(3, 7)
self.camLens.setFilmSize(Vec2(0.030, 0.030))
self.render.setAntialias(AntialiasAttrib.MAuto)
self.render.setDepthOffset(1)
self.render.setShaderAuto()
self.setupScene(client)
self.setupLights()
# setup filters
if args.ambient_occlusion:
filters = CommonFilters(self.win, self.cam)
filters.setAmbientOcclusion()
# setup periodic tasks
self.time = 0
self.taskMgr.add(self.spinCameraTask, "SpinCameraTask")
self.taskMgr.add(self.stepSimulationTask, "StepSimulationTask")
if args.debug:
self.oobe()
def __init__(self,
client,
task='peg-in-hole',
task_num=1,
offset=[0, 0, 0],
args=None,
is_test=False):
assert task in TASK_LIST, "Please regisiter your custom env first!"
assert (task_num == 1 or
(task_num > 1 and offset != [0, 0, 0])), "Offset is in valid."
self.task = task
self.task_num = task_num
self.offset = offset
self.args = args
self.sub_env = TASK_LIST[self.task]
self.p = BulletClient(client)
self.p.resetDebugVisualizerCamera(
cameraDistance=1.5,
cameraYaw=0,
cameraPitch=-40,
cameraTargetPosition=[0.55, -0.35, 0.2])
self.sub_envs = []
self._create_env()
self.action_space = MultiAgentActionSpace(
[self.sub_env.action_space for i in range(task_num)])
self.observation_space = MultiAgentObservationSpace(
[self.sub_env.observation_space for i in range(task_num)])
# test_mode
self.is_test = is_test
def __init__(self, engine, frame_size):
self._engine = engine
self._frame_size = frame_size
self._client = BulletClient(pb.DIRECT)
self._client.setAdditionalSearchPath(pybullet_data.getDataPath())
self._prepare_render(engine, frame_size)
self._prepare_scene()
def _apply_force(bc: BulletClient, obj, force: Sequence[float]):
force = _force_multiplier * np.array([*force, _zforce])
obj_pos, _ = bc.getBasePositionAndOrientation(obj)
bc.applyExternalForce(objectUniqueId=obj,
linkIndex=-1,
forceObj=force,
posObj=obj_pos,
flags=pybullet.WORLD_FRAME)
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()
class BaseTestCase(unittest.TestCase):
def setUp(self):
self.client = BulletClient(pb.DIRECT)
self.client.setAdditionalSearchPath(pybullet_data.getDataPath())
self.render = RendererMock()
self.plugin = RenderingPlugin(self.client, self.render)
def tearDown(self):
self.plugin.unload()
self.client.disconnect()
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 joint_angles_from_link_positions(
pybullet_client: bullet_client.BulletClient,
urdf_id: int,
link_ids: Sequence[int],
link_positions: Sequence[Sequence[float]],
positions_are_in_world_frame: bool = False,
joint_dof_ids: Optional[Sequence[int]] = None,
) -> np.ndarray:
"""Uses Inverse Kinematics to calculate joint angles.
Args:
pybullet_client: The bullet client.
urdf_id: The unique id returned after loading URDF.
link_ids: The link ids to compute the IK.
link_positions: The (x, y, z) of the links in the body or the world frame,
depending on whether the argument link_position_in_world_frame is true.
positions_are_in_world_frame: Whether the input link positions are specified
in the world frame or the robot's base frame.
joint_dof_ids: The degrees of freedom index of the joints we want to extract
the angles. This can be different from the joint unique ids. For example,
a fixed joint will increase the joint unique id but will not increase the
number of degree of freedoms. The joint dof id can be extracted in
PyBullet by getJointInfo, which corresponds to the "qIndex" in the
returned values. If not specified, will return all movable joint angles.
Returns:
A list of joint angles.
"""
if positions_are_in_world_frame:
world_link_positions = link_positions
else:
# The PyBullet inverse Kinematics Calculation depends on the current URDF
# position/orientation, and we cannot pass them to the API. So we have to
# always query the current base position/orientation to compute world frame
# link positions.
urdf_base_position, urdf_base_orientation = (
pybullet_client.getBasePositionAndOrientation(urdf_id))
world_link_positions = []
for link_position in link_positions:
world_link_position, _ = pybullet_client.multiplyTransforms(
urdf_base_position, urdf_base_orientation, link_position,
_IDENTITY_ROTATION_QUAT)
world_link_positions.append(world_link_position)
# Notice that the API expects the link positions in the world frame.
all_joint_angles = pybullet_client.calculateInverseKinematics2(
urdf_id, link_ids, world_link_positions, solver=_IK_SOLVER_TYPE)
# Extract the relevant joint angles.
if joint_dof_ids is None:
return np.array(all_joint_angles)
return np.array([all_joint_angles[i] for i in joint_dof_ids])
def load_scene(bullet: BulletClient, model_path: str,
simulation_config: SimulationConfig) -> Scene:
scene = Scene(bullet)
object_ids = [
bullet.loadURDF(model_path,
flags=simulation_config.model_load_flags)
]
for body_id in object_ids:
body_info = BodyInfo(*bullet.getBodyInfo(body_id))
body_item = Item(bullet,
name=body_info.body_name.decode("utf8"),
body_id=body_id,
link_index=None)
scene.add_item(body_item)
for joint_index in range(bullet.getNumJoints(body_id)):
joint_info = JointInfo(
*bullet.getJointInfo(body_item.body_id, joint_index))
joint_item = Item(bullet,
name=joint_info.link_name.decode("utf8"),
body_id=body_id,
link_index=joint_index)
joint = Joint(bullet,
item=joint_item,
name=joint_info.joint_name.decode("utf8"),
body_id=body_id,
joint_index=joint_index,
joint_type=joint_info.joint_type,
damping=joint_info.joint_damping,
friction=joint_info.joint_friction,
max_force=joint_info.joint_max_force,
lower_limit=joint_info.joint_lower_limit,
upper_limit=joint_info.joint_upper_limit,
max_velocity=joint_info.joint_max_velocity)
scene.add_item(joint_item)
if joint.joint_type == pybullet.JOINT_REVOLUTE:
scene.add_joint(joint)
elif joint.joint_type == pybullet.JOINT_FIXED:
pass
else:
raise AssertionError(
f'Only revolute and fixed joints are supported atm, got: {joint_info}'
)
return scene
def __init__(
self,
pose: Pose,
speed: float,
dimensions: BoundingBox,
bullet_client: bc.BulletClient,
):
self._dimensions = dimensions
self._bullet_body = BulletBoxShape(self._dimensions.as_lwh,
bullet_client)
self._bullet_constraint = BulletPositionConstraint(
self._bullet_body, bullet_client)
bullet_client.setCollisionFilterGroupMask(self._bullet_body._bullet_id,
-1, 0x0, 0x0)
self.control(pose, speed)
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_robot_description_from_sdf(abs_urdf_path: str, sim: BulletClient):
"""Loads a SDF file into the simulation."""
log.info("loading sdf file: {}".format(abs_urdf_path))
robot_id = sim.loadSDF(abs_urdf_path, )[0]
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
world_id = pybullet.loadURDF("plane.urdf", [0.0, 0.0, 0.0])
return world_id, robot_id
def __init__(self, visualize=False, labyrinth=SimpleLabyrinthConfig()):
min_vel, max_vel = (-np.inf, np.inf)
min_time, max_time = (-1, 1)
self.observation_space = gym.spaces.Dict(
spaces={
"observation":
gym.spaces.Box(low=np.array([min_vel, min_vel, min_time]),
high=np.array([max_vel, max_vel, max_time])),
"desired_goal":
gym.spaces.Box(low=-1, high=1, shape=(2, )),
"achieved_goal":
gym.spaces.Box(low=-1, high=1, shape=(2, ))
})
self._visualize = visualize
if visualize:
assert not self._viz_lock_taken, "only one PyBullet simulation can be visualized simultaneously"
PyBullet._viz_lock_taken = True
self._bullet = BulletClient(
connection_mode=pybullet.GUI if visualize else pybullet.DIRECT)
self._bullet.setAdditionalSearchPath(
pybullet_data.getDataPath()) # used by loadURDF
self._bullet.setGravity(0, 0, -9.81)
self._bullet.loadURDF("plane.urdf")
self._agent_ball = self._bullet.loadURDF(self._red_ball_fname,
labyrinth.agent_initial_pos)
self._goal_ball = self._bullet.loadURDF(self._green_ball_fname,
labyrinth.goal_initial_pos,
useFixedBase=1)
labyrinth_fname = os.path.join(self.__filelocation__, labyrinth.fname)
self._bullet.loadURDF(labyrinth_fname,
labyrinth.position,
useFixedBase=1)
self._arrow = self._bullet.loadURDF(self._arrow_fname,
labyrinth.arrow_initial_pos,
useFixedBase=1)
self._ball_radius = labyrinth.ball_radius
self._norm, self._denorm = normalizer(labyrinth.lower_bound,
labyrinth.upper_bound)
self.normed_starting_agent_obs = self._norm(
labyrinth.agent_initial_pos[:2])
self._cam_settings = labyrinth.getCamSettings(self._bullet)
self._goal_img = None
def rotate_to_base_frame(
pybullet_client: bullet_client.BulletClient,
urdf_id: int,
vector: Sequence[float],
init_orientation_inv_quat: Optional[Sequence[float]] = (0, 0, 0, 1)
) -> np.ndarray:
"""Rotates the input vector to the base coordinate systems.
Note: This is different from world frame to base frame transformation, as we
do not apply any translation here.
Args:
pybullet_client: The bullet client.
urdf_id: The unique id returned after loading URDF.
vector: Input vector in the world frame.
init_orientation_inv_quat:
Returns:
A rotated vector in the base frame.
"""
_, base_orientation_quat = (
pybullet_client.getBasePositionAndOrientation(urdf_id))
_, base_orientation_quat_from_init = pybullet_client.multiplyTransforms(
positionA=(0, 0, 0),
orientationA=init_orientation_inv_quat,
positionB=(0, 0, 0),
orientationB=base_orientation_quat)
_, inverse_base_orientation = pybullet_client.invertTransform(
[0, 0, 0], base_orientation_quat_from_init)
# PyBullet transforms requires simple list/tuple or it may crash.
if isinstance(vector, np.ndarray):
vector_list = vector.tolist()
else:
vector_list = vector
local_vector, _ = pybullet_client.multiplyTransforms(
positionA=(0, 0, 0),
orientationA=inverse_base_orientation,
positionB=vector_list,
orientationB=(0, 0, 0, 1),
)
return np.array(local_vector)
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()
class Connection(object):
def __init__(self, mode=Mode.DIRECT):
self.client = BulletClient(connection_mode=mode.to_bullet())
def info(self) -> ConnectionInfo:
result = self.client.getConnectionInfo()
is_connected = bool(result['isConnected'])
method = Mode(result['connectionMethod'])
return ConnectionInfo(is_connected, method)
def is_connected(self) -> bool:
return self.info().is_connected
def mode(self) -> Mode:
return Mode(self.info().method)
def disconnect(self):
self.client.disconnect()
def __del__(self):
self.client.disconnect()
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 load_robot_description_from_urdf(abs_urdf_path: str,
sim: BulletClient):
"""Loads a URDF file into the simulation."""
log.info("loading urdf file: {}".format(abs_urdf_path))
robot_id = sim.loadURDF(
abs_urdf_path,
basePosition=[0.0, 0.0, 0.0],
useFixedBase=True,
flags=pybullet.URDF_USE_INERTIA_FROM_FILE,
)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
world_id = pybullet.loadURDF("plane.urdf", [0.0, 0.0, 0.0])
return world_id, robot_id
def link_position_in_base_frame(
pybullet_client: bullet_client.BulletClient,
urdf_id: int,
link_id: int,
base_link_id: Optional[int] = None,
):
"""Computes the link's local position in the robot frame.
Args:
pybullet_client: The bullet client.
urdf_id: The unique id returned after loading URDF.
link_id: The link to calculate its relative position.
base_link_id: The link id of the base. For the kinematics robot, such as
wheeled_robot_base, three additional joints are added to connect the world
and the base. The base_link_id is no longer -1, and need to be passed in.
Returns:
The relative position of the link.
"""
if base_link_id is None:
base_position, base_orientation = (
pybullet_client.getBasePositionAndOrientation(urdf_id))
else:
base_link_state = pybullet_client.getLinkState(urdf_id, base_link_id)
base_position, base_orientation = base_link_state[0], base_link_state[
1]
inverse_translation, inverse_rotation = pybullet_client.invertTransform(
base_position, base_orientation)
link_state = pybullet_client.getLinkState(urdf_id, link_id)
link_position = link_state[0]
link_local_position, _ = pybullet_client.multiplyTransforms(
inverse_translation, inverse_rotation, link_position, (0, 0, 0, 1))
return np.array(link_local_position)
def __init__(self):
BaseRenderer.__init__(self) # <- important
client = BulletClient(pb.DIRECT)
client.setAdditionalSearchPath(pybullet_data.getDataPath())
self.client = client
# bind external renderer
plugin = RenderingPlugin(client, self)
# setup scene
self.nodes = {}
self.render = vtk.vtkRenderer()
self.renWin = vtk.vtkRenderWindow()
self.renWin.AddRenderer(self.render)
colors = vtk.vtkNamedColors()
self.render.SetBackground(colors.GetColor3d("cobalt_green"))
self.setupScene(client)
# self.setupLights()
self.time = 0
iren = vtk.vtkRenderWindowInteractor()
iren.SetRenderWindow(self.renWin)
# Enable user interface interactor
iren.Initialize()
iren.AddObserver('TimerEvent', self.stepSimulationTask)
iren.CreateRepeatingTimer(50)
self.iren = iren
camera = self.render.GetActiveCamera()
camera.SetPosition([0, -3, 3])
# camera.SetFocalPoint(v)
# camera.SetViewUp(v)
camera.SetViewAngle(40)
def link_position_in_world_frame(
pybullet_client: bullet_client.BulletClient,
urdf_id: int,
link_id: int,
):
"""Computes the link's position in the world frame.
Args:
pybullet_client: The bullet client.
urdf_id: The unique id returned after loading URDF.
link_id: The link id to calculate its position.
Returns:
The position of the link in the world frame.
"""
return np.array(pybullet_client.getLinkState(urdf_id, link_id)[0])
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 compute_jacobian(
pybullet_client: bullet_client.BulletClient,
urdf_id: int,
link_id: int,
all_joint_positions: Sequence[float],
additional_translation: Optional[Sequence[float]] = (0, 0, 0),
) -> np.ndarray:
"""Computes the Jacobian matrix for the given point on a link.
CAVEAT: If during URDF loading process additional rotations are provided, the
computed Jacobian are also transformed.
Args:
pybullet_client: The bullet client.
urdf_id: The unique id returned after loading URDF.
link_id: The link id as returned from loadURDF.
all_joint_positions: all the joint positions of the robot. This should
include the dummy/kinematic drive joints for the wheeled robot.
additional_translation: The additional translation of the point in the link
CoM frame.
Returns:
The 3 x N transposed Jacobian matrix. where N is the total DoFs of the
robot. For a quadruped, the first 6 columns of the matrix corresponds to
the CoM translation and rotation. The columns corresponds to a leg can be
extracted with indices [6 + leg_id * 3: 6 + leg_id * 3 + 3].
"""
zero_vec = [0] * len(all_joint_positions)
jv, _ = pybullet_client.calculateJacobian(urdf_id,
link_id,
additional_translation,
all_joint_positions,
objVelocities=zero_vec,
objAccelerations=zero_vec)
jacobian = np.array(jv)
assert jacobian.shape[0] == 3
return jacobian
frame {FrameData} -- output image buffer
"""
self._viewer.render_lock.acquire()
for uid, node in self._node_dict.items():
pose = np.asarray(scene_state.matrix(uid)).reshape(4, 4).T
self._scene.set_pose(node, pose)
self._viewer.render_lock.release()
return False
def close(self):
return self._viewer.close_external()
if __name__ == "__main__":
client = BulletClient(pb.DIRECT)
client.setAdditionalSearchPath(pybullet_data.getDataPath())
grav = np.array([0., 0., -9.91])
client.setGravity(*grav)
gui = PyrenderGUI(client)
plane = client.loadURDF("plane.urdf", useMaximalCoordinates=True)
table = client.loadURDF("table/table.urdf",
basePosition=[0.4, 0., 0.],
flags=pb.URDF_USE_MATERIAL_COLORS_FROM_MTL)
ball = client.loadURDF("soccerball.urdf",
basePosition=[0, 0, 1],
globalScaling=.25)
laikago = client.loadURDF("laikago/laikago.urdf",
basePosition=[0, 1.2, 0.5],
baseOrientation=[1, 0, 0, 1])
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
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])
def run_test(num_frames, frame_size, engine, fps_out):
"""Compute fps of a specific engine."""
client = BulletClient(pb.DIRECT)
client.setAdditionalSearchPath(pybullet_data.getDataPath())
# load renderer
if 'tiny' == engine:
pass
elif 'egl' == engine:
egl = pkgutil.get_loader('eglRenderer')
client.loadPlugin(egl.get_filename(), "_eglRendererPlugin")
elif 'panda3d' == engine:
RenderingPlugin(client, P3dRenderer(multisamples=4))
elif 'pyrender' == engine:
RenderingPlugin(client, PyrRenderer(platform='egl'))
# sample scene
table = client.loadURDF("table/table.urdf",
flags=pb.URDF_USE_MATERIAL_COLORS_FROM_MTL)
client.resetBasePositionAndOrientation(table, [0.4, 0.04, -0.7],
[0, 0, 0, 1])
kuka = client.loadSDF("kuka_iiwa/kuka_with_gripper2.sdf")[0]
client.resetBasePositionAndOrientation(kuka, [0.0, 0.0, 0.0], [0, 0, 0, 1])
client.resetJointState(kuka, 3, -1.57)
proj_mat = pb.computeProjectionMatrixFOV(fov=60,
aspect=1.0,
nearVal=0.1,
farVal=10)
view_mat = pb.computeViewMatrixFromYawPitchRoll((0.4, 0, 0), 2.0, 0, -40,
0, 2)
# warming up
_, _, color, depth, mask = client.getCameraImage(*frame_size,
projectionMatrix=proj_mat,
viewMatrix=view_mat)
if engine in ('tiny', 'egl'):
depth = depth_from_zbuffer(depth, 0.1, 10.0)
img = Image.fromarray(color[:, :, :3])
img.save(f'color_{engine}.png')
img = Image.fromarray(((depth / depth.max()) * 255).astype(np.uint8))
img.save(f'depth_{engine}.png')
img = Image.fromarray(mask.astype(np.uint16))
img.save(f'mask_{engine}.png')
# compute fps
start = timer()
for _ in range(num_frames):
client.getCameraImage(*frame_size,
projectionMatrix=proj_mat,
viewMatrix=view_mat)
end = timer()
fps_out.value = num_frames / (end - start)
def __init__(self, mode=Mode.DIRECT):
self.client = BulletClient(connection_mode=mode.to_bullet())
def setUp(self):
self.client = BulletClient(pb.DIRECT)
self.client.setAdditionalSearchPath(pybullet_data.getDataPath())
self.render = RendererMock()
self.plugin = RenderingPlugin(self.client, self.render)
self.random = np.random.RandomState(77)
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