So here is what the FrankaDeformableDemo.py did, I believe it’s all on PhysX.
import carb
import math
import numpy as np
import omni
from pxr import Gf, Sdf, UsdGeom, UsdShade, PhysxSchema, UsdPhysics
import omni.physxdemos as demo
from omni.physx.scripts import deformableUtils, utils, physicsUtils
from omni.physxdemos.utils import franka_helpers
from omni.physxdemos.utils import numpy_utils
class FrankaDeformableDemo(demo.AsyncDemoBase):
title = "Franka Deformable"
category = demo.Categories.COMPLEX_SHOWCASES
short_description = "Demo showing Franka robot arms interacting with deformable cubes"
description = "The Franka robot arms use Jacobian-based inverse kinematics to lift and drop the red cube in a bowl of deformable cubes."
kit_settings = {
"persistent/app/viewport/displayOptions": demo.get_viewport_minimal_display_options_int(),
}
params = {"Num_Frankas": demo.IntParam(4, 1, 20, 1), "Num_Greens": demo.IntParam(9, 0, 9, 1)}
demo_camera = Sdf.Path("/World/Camera")
def __init__(self):
super().__init__(enable_tensor_api=True, enable_fabric=False)
self._reset_hydra_instancing_on_shutdown = False
# when looping, box will be dropped after lifting and picking action will repeat
self.loop = True
self.asset_paths = {
"franka": demo.get_demo_asset_path("FrankaDeformable/SubUSDs/Franka/franka_alt_fingers.usd"),
"table": demo.get_demo_asset_path("FrankaDeformable/SubUSDs/Table_Rounded/Table_Rounded.usd"),
"stool": demo.get_demo_asset_path("FrankaDeformable/SubUSDs/Franka_Stool/Franka_Stool.usd"),
"cube": demo.get_demo_asset_path("FrankaDeformable/SubUSDs/box_high.usd"),
"bowl": demo.get_demo_asset_path("FrankaDeformable/SubUSDs/bowl_plate.usd"),
"cube_materials": demo.get_demo_asset_path("FrankaDeformable/SubUSDs/deformable_cubes_materials.usd"),
}
self.demo_base_usd_url = demo.get_demo_asset_path("FrankaDeformable/StagingDeformable.usd")
def on_startup(self):
sceneGraphInstancingEnabled = carb.settings.get_settings().get("/persistent/omnihydra/useSceneGraphInstancing")
if not sceneGraphInstancingEnabled:
carb.settings.get_settings().set("/persistent/omnihydra/useSceneGraphInstancing", True)
self._reset_hydra_instancing_on_shutdown = True
def create_jello_cube(self, stage, path, name, position, size, mesh_path, phys_material_path, grfx_material):
cube_path = path.AppendChild(name)
stage.DefinePrim(cube_path).GetReferences().AddReference(mesh_path)
skinMesh = UsdGeom.Mesh.Define(stage, cube_path)
skinMesh.AddTranslateOp().Set(position)
skinMesh.AddOrientOp().Set(Gf.Quatf(1.0))
skinMesh.AddScaleOp().Set(Gf.Vec3f(size, size, size))
deformableUtils.add_physx_deformable_body(
stage,
cube_path,
simulation_hexahedral_resolution=3,
collision_simplification=True,
self_collision=False,
solver_position_iteration_count=self.pos_iterations,
)
physicsUtils.add_physics_material_to_prim(stage, skinMesh.GetPrim(), phys_material_path)
physxCollisionAPI = PhysxSchema.PhysxCollisionAPI.Apply(skinMesh.GetPrim())
physxCollisionAPI.GetContactOffsetAttr().Set(0.02)
physxCollisionAPI.CreateRestOffsetAttr().Set(0.001)
omni.kit.commands.execute(
"BindMaterialCommand", prim_path=cube_path, material_path=grfx_material, strength=None
)
def create(self, stage, Num_Frankas, Num_Greens):
self.defaultPrimPath = stage.GetDefaultPrim().GetPath()
self.stage = stage
self.num_envs = Num_Frankas
self.num_greens = Num_Greens
# Physics scene
scene = UsdPhysics.Scene.Define(stage, self.defaultPrimPath.AppendChild("physicsScene"))
scene.CreateGravityDirectionAttr().Set(Gf.Vec3f(0.0, 0.0, -1.0))
scene.CreateGravityMagnitudeAttr().Set(9.807)
utils.set_physics_scene_asyncsimrender(scene.GetPrim())
physxSceneAPI = PhysxSchema.PhysxSceneAPI.Apply(scene.GetPrim())
physxSceneAPI.CreateFrictionOffsetThresholdAttr().Set(0.001)
physxSceneAPI.CreateFrictionCorrelationDistanceAttr().Set(0.0005)
physxSceneAPI.CreateGpuTotalAggregatePairsCapacityAttr().Set(10 * 1024)
physxSceneAPI.CreateGpuFoundLostPairsCapacityAttr().Set(10 * 1024)
physxSceneAPI.CreateGpuCollisionStackSizeAttr().Set(64 * 1024 * 1024)
physxSceneAPI.CreateGpuFoundLostAggregatePairsCapacityAttr().Set(10 * 1024)
# clamp iterations:
self.pos_iterations = 20
self.vel_iterations = 1
physxSceneAPI.GetMaxPositionIterationCountAttr().Set(self.pos_iterations)
UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z)
UsdGeom.SetStageMetersPerUnit(stage, 1.0)
# setup ground collision plane:
utils.addPlaneCollider(stage, "/World/physicsGroundPlaneCollider", "Z")
# deformable material path
deformable_material_path = omni.usd.get_stage_next_free_path(stage, "/DeformableBodyMaterial", True)
deformableUtils.add_deformable_body_material(
stage,
deformable_material_path,
youngs_modulus=10000000.0,
poissons_ratio=0.499,
damping_scale=0.0,
elasticity_damping=0.0001,
dynamic_friction=1.0,
density=300,
)
deformable_material2_path = omni.usd.get_stage_next_free_path(stage, "/DeformableBodyMaterial2", True)
deformableUtils.add_deformable_body_material(
stage,
deformable_material2_path,
youngs_modulus=4000000.0,
poissons_ratio=0.499,
damping_scale=0.0,
elasticity_damping=0.005,
dynamic_friction=0.05,
density=100,
)
# Franka finger friction
mat_prim = UsdShade.Material.Define(stage, "/World/FrankaFingerPhysicsMaterial")
finger_material = UsdPhysics.MaterialAPI.Apply(mat_prim.GetPrim())
finger_material.CreateStaticFrictionAttr().Set(1.0)
finger_material.CreateDynamicFrictionAttr().Set(1.0)
# position parameters
table_scale = 0.8
table_height = 0.762 * table_scale
table_position = Gf.Vec3f(0.5, 0.0, 0.0)
env_spacing = 2
num_envs_per_side = max(1, round(math.sqrt(self.num_envs)))
row_half_length = (num_envs_per_side - 1) * env_spacing * 0.5
# create deformable cube parameters
self.box_size = 0.05
box_loc = Gf.Vec3f(table_position) + Gf.Vec3f(0.0, 0.0, table_height + self.box_size * 3.5)
# Position Camera Based on Scene Size
cam = UsdGeom.Camera.Define(stage, self.demo_camera)
cam_height = 1.1
location = Gf.Vec3f(row_half_length + 6.5, row_half_length + 4.0, cam_height)
target = Gf.Vec3f(0.0, 0.0, cam_height - 0.4)
delta = target - location
rotZ = math.degrees(math.atan2(-delta[0], delta[1]))
rotX = math.degrees(math.atan2(delta[2], math.sqrt(delta[0] ** 2.0 + delta[1] ** 2.0)))
rotZQ = Gf.Quatf(Gf.Rotation(Gf.Vec3d([0, 0, 1]), rotZ).GetQuat())
rotXQ = Gf.Quatf(Gf.Rotation(Gf.Vec3d([1, 0, 0]), rotX + 90).GetQuat())
physicsUtils.setup_transform_as_scale_orient_translate(cam)
physicsUtils.set_or_add_translate_op(cam, translate=location)
physicsUtils.set_or_add_orient_op(cam, orient=rotZQ * rotXQ)
cam.CreateClippingRangeAttr(Gf.Vec2f(0.01, 100))
# get materials for deformable cubes
looks_path = self.defaultPrimPath.AppendChild("Looks")
stage.DefinePrim(looks_path).GetReferences().AddReference(self.asset_paths["cube_materials"], "/Looks")
green_glass_path = looks_path.AppendChild("GreenGlass")
red_glass_path = looks_path.AppendChild("RedGlass")
# drive and default dof pos:
self.default_dof_pos, drive_params = self.get_franka_parameters()
# create grid of frankas and tables:
envsScopePath = self.defaultPrimPath.AppendPath("envs")
UsdGeom.Scope.Define(stage, envsScopePath)
for i in range(self.num_envs):
# create env:
col_number = i % num_envs_per_side
row_number = i // num_envs_per_side
x_pos = -row_half_length + col_number * env_spacing
y_pos = -row_half_length + row_number * env_spacing
env_xform = UsdGeom.Xform.Define(stage, envsScopePath.AppendChild(f"env_{i}"))
env_xform.AddTranslateOp().Set(Gf.Vec3f(x_pos, y_pos, 0.0))
# add franka and its stool
franka_stool_position = Gf.Vec3f(-0.027, 0.0, -0.001)
stool_path = env_xform.GetPath().AppendChild("franka_stool")
stage.DefinePrim(stool_path).GetReferences().AddReference(self.asset_paths["stool"])
stool_xform = UsdGeom.Xform.Get(stage, stool_path)
utils.setStaticCollider(stool_xform.GetPrim(), approximationShape="boundingCube")
assert stool_xform
physicsUtils.set_or_add_translate_op(stool_xform, translate=franka_stool_position)
franka_path = env_xform.GetPath().AppendChild("franka")
assert stage.DefinePrim(franka_path).GetReferences().AddReference(self.asset_paths["franka"], "/panda")
franka_xform = UsdGeom.Xform.Get(stage, franka_path)
assert franka_xform
physicsUtils.set_or_add_translate_op(franka_xform, translate=Gf.Vec3f(0.0, 0.0, table_height - 0.4))
physicsUtils.set_or_add_scale_op(franka_xform, scale=Gf.Vec3f(0.01))
# setup drives:
franka_helpers.configure_franka_drives(stage, franka_path, self.default_dof_pos, drive_params)
# setup iterations:
physxArticulationAPI = PhysxSchema.PhysxArticulationAPI.Apply(franka_xform.GetPrim())
physxArticulationAPI.GetSolverPositionIterationCountAttr().Set(self.pos_iterations)
physxArticulationAPI.GetSolverVelocityIterationCountAttr().Set(self.vel_iterations)
# setup fingers
prim = stage.GetPrimAtPath(str(env_xform.GetPath()) + "/franka/panda_leftfinger/geometry")
bindingAPI = UsdShade.MaterialBindingAPI.Apply(prim)
bindingAPI.Bind(mat_prim, UsdShade.Tokens.weakerThanDescendants, "physics")
prim = stage.GetPrimAtPath(str(env_xform.GetPath()) + "/franka/panda_rightfinger/geometry")
bindingAPI = UsdShade.MaterialBindingAPI.Apply(prim)
bindingAPI.Bind(mat_prim, UsdShade.Tokens.weakerThanDescendants, "physics")
# set max joint velocity
joint = PhysxSchema.PhysxJointAPI.Get(
stage, str(env_xform.GetPath()) + "/franka/panda_hand/panda_finger_joint1"
)
joint.GetMaxJointVelocityAttr().Set(1)
joint = PhysxSchema.PhysxJointAPI.Get(
stage, str(env_xform.GetPath()) + "/franka/panda_hand/panda_finger_joint2"
)
joint.GetMaxJointVelocityAttr().Set(1)
# add table:
table_path = env_xform.GetPath().AppendPath("table")
assert stage.DefinePrim(table_path).GetReferences().AddReference(self.asset_paths["table"])
xform = UsdGeom.Xform.Get(stage, table_path)
utils.setStaticCollider(xform.GetPrim(), approximationShape="boundingCube")
physicsUtils.set_or_add_translate_op(xform, translate=table_position)
physicsUtils.set_or_add_scale_op(xform, scale=Gf.Vec3f(table_scale))
# add bowl:
bowl_path = env_xform.GetPath().AppendPath("bowl")
assert (
stage.DefinePrim(bowl_path).GetReferences().AddReference(self.asset_paths["bowl"], "/World/bowl_plate")
)
xform = UsdGeom.Xform.Get(stage, bowl_path)
xform.AddTranslateOp().Set(table_position + Gf.Vec3f(0.0, 0.0, table_height))
rigidbody_api = PhysxSchema.PhysxRigidBodyAPI.Apply(
stage.GetPrimAtPath(bowl_path.AppendChild("bowl_plate"))
)
rigidbody_api.CreateSolverPositionIterationCountAttr(self.pos_iterations)
rigidbody_api.CreateSolverVelocityIterationCountAttr(self.vel_iterations)
# create soft cube
self.create_jello_cube(
stage,
env_xform.GetPath(),
"box",
box_loc,
self.box_size,
self.asset_paths["cube"],
deformable_material_path,
red_glass_path,
)
# create extras
extras_path = env_xform.GetPath().AppendChild("extras")
ex_spacing = 1.5
for z in range(3):
for y in range(3):
for x in range(3):
if x == 1 & y == 1 & z == 1:
continue
j = x + y * 3 + z * 3 * 3
if j > 13:
j -= 1
if j >= self.num_greens:
break
self.create_jello_cube(
stage,
extras_path,
f"extra_{j}",
box_loc
+ Gf.Vec3f((x - 1) * ex_spacing, (y - 1) * ex_spacing, (z - 1) * ex_spacing)
* self.box_size,
self.box_size,
self.asset_paths["cube"],
deformable_material2_path,
green_glass_path,
)
omni.usd.get_context().get_selection().set_selected_prim_paths([], False)
def on_tensor_start(self, tensorApi):
sim = tensorApi.create_simulation_view("numpy")
sim.set_subspace_roots("/World/envs/*")
# franka view
self.frankas = sim.create_articulation_view("/World/envs/*/franka")
self.franka_indices = np.arange(self.frankas.count, dtype=np.int32)
# set default dof pos:
init_dof_pos = np.stack(self.num_envs * [np.array(self.default_dof_pos, dtype=np.float32)])
self.frankas.set_dof_positions(init_dof_pos, self.franka_indices)
# end effector view
self.hands = sim.create_rigid_body_view("/World/envs/*/franka/panda_hand")
# get initial hand transforms
init_hand_transforms = self.hands.get_transforms().copy()
self.init_pos = init_hand_transforms[:, :3]
self.init_rot = init_hand_transforms[:, 3:]
# self.boxes = sim.create_rigid_body_view("/World/envs/*/box")
self.boxes = sim.create_soft_body_view("/World/envs/*/box")
# box corner coords, used to determine grasping angle
box_half_size = 0.5 * self.box_size
corner_coord = np.array([box_half_size, box_half_size, box_half_size])
self.corners = np.stack(self.num_envs * [corner_coord])
# get initial box transforms
self.init_box_transforms = self.boxes.get_transforms().copy()
# downward direction
self.down_dir = np.array([0, 0, -1]).reshape(3, 1)
# hand orientation for grasping
self.down_q = np.stack(self.num_envs * [np.array([1.0, 0.0, 0.0, 0.0])])
# prevent garbage collector from deleting the sim
self.sim = sim
def on_shutdown(self):
self.frankas = None
self.hands = None
self.boxes = None
self.sim = None
# disable hydra instancing if they had to be enabled
if self._reset_hydra_instancing_on_shutdown:
carb.settings.get_settings().set("/persistent/omnihydra/useSceneGraphInstancing", False)
super().on_shutdown()
def on_physics_step(self, dt):
# get box transforms
box_transforms = self.boxes.get_transforms()
box_pos = box_transforms[:, :3]
# get end effector transforms
hand_transforms = self.hands.get_transforms()
hand_pos = hand_transforms[:, :3]
hand_rot = hand_transforms[:, 3:]
# get franka DOF states
dof_pos = self.frankas.get_dof_positions()
# get franka jacobians
jacobians = self.frankas.get_jacobians()
# direction from end effector to box
to_box = box_pos - hand_pos
box_dist = np.linalg.norm(to_box, axis=1)
box_dir = to_box / box_dist[:, None]
box_dot = (box_dir @ self.down_dir).flatten()
# how far the hand should be from box for grasping
grasp_offset = 0.10
# determine if we're holding the box (grippers are closed and box is near)
gripper_sep = dof_pos[:, 7] + dof_pos[:, 8]
gripped = (gripper_sep < self.box_size) & (box_dist < grasp_offset * 2.0)
# determine if the box is unreachable (too far from initial box position)
from_start = box_pos - self.init_box_transforms[:, :3]
start_dist = np.linalg.norm(from_start, axis=1)
unreachable = start_dist > 0.5
# if hand is above box, descend to grasp offset; otherwise, seek a position above the box
above_box = (box_dot >= 0.9) & (box_dist < grasp_offset * 3)
grasp_pos = box_pos.copy()
grasp_pos[:, 2] = np.where(above_box, box_pos[:, 2] + grasp_offset, box_pos[:, 2] + grasp_offset * 2.5)
# if box is gripped, return to initial pose; otherwise get into grasping pose
goal_pos = np.where(gripped[:, None], self.init_pos, grasp_pos)
goal_rot = self.down_q
# compute position and orientation error
pos_err = goal_pos - hand_pos
max_err = 0.05 # clamp max position error
pos_err_mag = (
np.linalg.norm(pos_err, axis=1)[:, None] + 0.0001
) # VR: add a tiny number to prevent division-by-zero warning
pos_err = np.where(pos_err_mag > max_err, pos_err * max_err / pos_err_mag, pos_err) # !!!
orn_err = orientation_error(goal_rot, hand_rot)
dpose = np.concatenate([pos_err, orn_err], -1)[:, None].transpose(0, 2, 1)
# jacobian entries corresponding to franka hand
franka_hand_index = 8 # !!!
j_eef = jacobians[:, franka_hand_index - 1, :]
# solve damped least squares
j_eef_T = np.transpose(j_eef, (0, 2, 1))
d = 0.05 # damping term
lmbda = np.eye(6) * (d**2)
u = (j_eef_T @ np.linalg.inv(j_eef @ j_eef_T + lmbda) @ dpose).reshape(self.num_envs, 9)
# update position targets
pos_targets = dof_pos + u # * 0.3
# close the grippers when we're near the box; if looping, open them when we return to starting position
if self.loop:
close_gripper = np.where(gripped, (box_dist < grasp_offset * 1.5), (box_dist < grasp_offset * 1.1))
else:
close_gripper = box_dist < grasp_offset
grip0 = 0.015
grip1 = 0.05
grip_acts = np.where(
close_gripper[:, None],
[[grip0, grip0]] * self.num_envs,
[[grip1, grip1]] * self.num_envs,
)
# pos_targets[:, 7:9] = pos_targets[:, 7:9] * 0.3 + grip_acts * 0.7
pos_targets[:, 7:9] = grip_acts
# reset the hand if the cube's unreachable
pos_targets = np.where(unreachable[:, None], self.default_dof_pos, pos_targets)
# apply position targets
self.frankas.set_dof_position_targets(pos_targets, self.franka_indices)
Here is how I did
import argparse
from isaacsim import SimulationApp
# create argparser
parser = argparse.ArgumentParser(description="dVRK in an empty environment")
parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to spawn.")
parser.add_argument("--headless", action="store_true", dest="headless")
parser.add_argument("--gpu", action="store_true", dest="gpu")
args = parser.parse_args()
headless = args.headless
gpu = args.gpu
num_envs = args.num_envs
if gpu:
device = "cuda"
backend = "torch"
else:
device = "cpu"
backend = "numpy"
# Initialize SimulationApp
simulation_app = SimulationApp({"headless": headless})
import omni
import torch
from omni.isaac.core.utils import stage
from omni.isaac.core.world import World
import omni.isaac.core.utils.prims as prims_utils
from omni.isaac.core.utils.semantics import add_update_semantics
from omni.physx.scripts import deformableUtils, physicsUtils, deformableMeshUtils, utils
from pxr import UsdGeom, UsdShade, Gf, PhysxSchema, UsdPhysics, Sdf, PhysicsSchemaTools
from omni.isaac.core.prims import XFormPrim, RigidPrim, RigidPrimView, XFormPrimView
def load_franka(world: World):
for i in range(num_envs):
robot_prim_path = f"/World/env_{i}/robot"
robot_name = f"franka_{i}"
position = np.array([i, 0.0, 0.0])
if device == "cuda":
position = torch.tensor(position, dtype=torch.float32).to(device)
franka = Franka(
prim_path=robot_prim_path,
name=robot_name,
position=position,
)
robots = ArticulationView(
prim_paths_expr="/World/env_*/robot",
name="franka_view",
)
world.scene.add(robots)
return robots
def load_deformable(world: World):
current_stage = omni.usd.get_context().get_stage()
for i in range(num_envs):
deformable_prim_path = f"/World/env_{i}/deformable"
deformable_name = f"tissue_{i}"
current_stage.DefinePrim(deformable_prim_path).GetReferences().AddReference(box_usd)
cube_mesh = UsdGeom.Mesh.Define(current_stage, deformable_prim_path)
cube_mesh.AddTranslateOp().Set(Gf.Vec3f(i+0.3893, 0.005, 0.005))
cube_mesh.AddScaleOp().Set(Gf.Vec3f(0.1, 0.2, 0.015))
cube_mesh.CreateDisplayColorAttr().Set([Gf.Vec3f(1, 1, 0)])
# Add PhysX deformable body
deformableUtils.add_physx_deformable_body(
current_stage,
# cube_mesh.GetPath(),
deformable_prim_path,
collision_simplification=True,
simulation_hexahedral_resolution=30, # The parameter controlling the resolution of the soft body simulation mesh
solver_position_iteration_count=20, # Number of the solver's positional iteration counts
self_collision=True,
)
# Create a physics material
deformable_material_path = omni.usd.get_stage_next_free_path(current_stage, "/DeformableBodyMaterial", True)
deformableUtils.add_deformable_body_material(
current_stage,
deformable_material_path,
youngs_modulus=1000.0,
poissons_ratio=0.3,
damping_scale=0.0,
elasticity_damping=0.0001,
dynamic_friction=0.5,
density=100,
)
# Assign the material to the mesh
physicsUtils.add_physics_material_to_prim(current_stage, cube_mesh.GetPrim(), deformable_material_path)
physxCollisionAPI = PhysxSchema.PhysxCollisionAPI.Apply(cube_mesh.GetPrim())
physxCollisionAPI.GetContactOffsetAttr().Set(0.02)
physxCollisionAPI.CreateRestOffsetAttr().Set(0.001)
add_update_semantics(prims_utils.get_prim_at_path(deformable_prim_path), deformable_name)
deformables = XFormPrimView(
prim_paths_expr="/World/env_*/deformable",
name="deformables_view",
)
world.scene.add(deformables)
return deformables
def run_simulator(world: World, deformables):
while simulation_app.is_running():
positions, orientations = deformables.get_world_poses()
print(positions, orientations)
def main():
# Create the World
world = World(backend=backend, device=device)
if device == "cuda":
world.get_physics_context().enable_gpu_dynamics(True)
world.scene.add_default_ground_plane()
robots = load_franka(world)
deformables = load_deformable(world)
# initialize physics
world.initialize_physics()
# Reset the world and start simulation
world.reset()
print("[INFO]: Setup complete...")
# Run the simulation
run_simulator(world, deformables)
# Close the simulation app
simulation_app.close()
if __name__ == "__main__":
main()
So no matter how I changed the deformable object in the simulator (move it using cursor or grasp it using robot), the output of its pose won’t change.