hofee/gen_data_curobo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add isaac sim 2023 support and dockerfiles

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2023-11-04 09:32:30 -07:00
parent f2eb5f937a
commit 102c5d6ab2
41 changed files with 1284 additions and 622 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
examples/isaac_sim/batch_collision_checker.py
View File

@@ -17,51 +17,48 @@ a = torch.zeros(4, device="cuda:0")
# Standard Library
# Standard Library
import argparse
# Third Party
from omni.isaac.kit import SimulationApp
# CuRobo
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.util_file import get_world_configs_path, join_path, load_yaml
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
args = parser.parse_args()
simulation_app = SimulationApp(
{
"headless": False,
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Third Party
from omni.isaac.core.utils.extensions import enable_extension
ext_list = [
"omni.kit.asset_converter",
# "omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
]
[enable_extension(x) for x in ext_list]
# Third Party
import carb
import numpy as np
from helper import add_extensions
from omni.isaac.core import World
from omni.isaac.core.materials import OmniPBR
from omni.isaac.core.objects import cuboid, sphere
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.objects import sphere
# CuRobo
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import get_world_configs_path, join_path, load_yaml
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
########### OV #################
def main():
@@ -79,10 +76,7 @@ def main():
stage.SetDefaultPrim(xform)
stage.DefinePrim("/curobo", "Xform")
# my_world.stage.SetDefaultPrim(my_world.stage.GetPrimAtPath("/World"))
stage = my_world.stage
# stage.SetDefaultPrim(stage.GetPrimAtPath("/World"))
# Make a target to follow
target_list = []
target_material_list = []
offset_x = 3.5
@@ -131,6 +125,7 @@ def main():
x_sph = torch.zeros((n_envs, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
x_sph[..., 3] = radius
env_query_idx = torch.arange(n_envs, device=tensor_args.device, dtype=torch.int32)
add_extensions(simulation_app, args.headless_mode)
while simulation_app.is_running():
my_world.step(render=True)
if not my_world.is_playing():

88
examples/isaac_sim/batch_motion_gen_reacher.py
View File

@@ -17,62 +17,18 @@ a = torch.zeros(4, device="cuda:0")
# Standard Library
# Third Party
from omni.isaac.kit import SimulationApp
# CuRobo
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.state import JointState
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
simulation_app = SimulationApp(
{
"headless": False,
"width": "1920",
"height": "1080",
}
)
# Third Party
from omni.isaac.core.utils.extensions import enable_extension
ext_list = [
"omni.kit.asset_converter",
# "omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
]
[enable_extension(x) for x in ext_list]
# Standard Library
import argparse
# Third Party
import carb
import numpy as np
from helper import add_robot_to_scene
from omni.isaac.core import World
from omni.isaac.core.materials import OmniPBR
from omni.isaac.core.objects import cuboid, sphere
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.types import ArticulationAction
# CuRobo
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless", action="store_true", help="When True, enables headless mode", default=False
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
parser.add_argument(
"--visualize_spheres",
action="store_true",
@@ -83,6 +39,37 @@ parser.add_argument(
parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
args = parser.parse_args()
simulation_app = SimulationApp(
{
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Third Party
import carb
import numpy as np
from helper import add_extensions, add_robot_to_scene
from omni.isaac.core import World
from omni.isaac.core.objects import cuboid
########### OV #################
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.kit import SimulationApp
# CuRobo
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.state import JointState
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
def main():
usd_help = UsdHelper()
@@ -180,6 +167,7 @@ def main():
enable_graph=False, warmup_js_trajopt=False, batch=n_envs, batch_env_mode=True
)
add_extensions(simulation_app, args.headless_mode)
config = RobotWorldConfig.load_from_config(
robot_file, world_cfg_list, collision_activation_distance=act_distance
)

75
examples/isaac_sim/collision_checker.py
View File

@@ -17,60 +17,52 @@ a = torch.zeros(4, device="cuda:0")
# Standard Library
# Standard Library
import argparse
# Third Party
from omni.isaac.kit import SimulationApp
parser = argparse.ArgumentParser()
parser.add_argument(
"--nvblox", action="store_true", help="When True, enables headless mode", default=False
)
parser.add_argument(
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
args = parser.parse_args()
simulation_app = SimulationApp(
{
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Third Party
import carb
import numpy as np
from helper import add_extensions
from omni.isaac.core import World
from omni.isaac.core.materials import OmniPBR
from omni.isaac.core.objects import sphere
# CuRobo
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.util_file import get_world_configs_path, join_path, load_yaml
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
simulation_app = SimulationApp(
{
"headless": False,
"width": "1920",
"height": "1080",
}
)
# Third Party
from omni.isaac.core.utils.extensions import enable_extension
ext_list = [
"omni.kit.asset_converter",
# "omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
]
[enable_extension(x) for x in ext_list]
# Standard Library
import argparse
# Third Party
import carb
import numpy as np
from omni.isaac.core import World
from omni.isaac.core.materials import OmniPBR
from omni.isaac.core.objects import cuboid, sphere
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.types import ArticulationAction
# CuRobo
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
parser = argparse.ArgumentParser()
parser.add_argument(
"--nvblox", action="store_true", help="When True, enables headless mode", default=False
)
args = parser.parse_args()
from curobo.util_file import get_world_configs_path, join_path, load_yaml
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
def draw_line(start, gradient):
@@ -154,6 +146,7 @@ def main():
x_sph = torch.zeros((1, 1, 1, 4), device=tensor_args.device, dtype=tensor_args.dtype)
x_sph[..., 3] = radius
add_extensions(simulation_app, args.headless_mode)
while simulation_app.is_running():
my_world.step(render=True)
if not my_world.is_playing():

46
examples/isaac_sim/helper.py
View File

@@ -20,15 +20,43 @@ from omni.isaac.core.materials import OmniPBR
from omni.isaac.core.objects import cuboid
from omni.isaac.core.robots import Robot
# CuRobo
from curobo.util.logger import log_warn
ISAAC_SIM_23 = False
try:
# Third Party
from omni.isaac.urdf import _urdf # isaacsim 2022.2
except ImportError:
# Third Party
from omni.importer.urdf import _urdf # isaac sim 2023.1
ISAAC_SIM_23 = True
# Standard Library
from typing import Optional
# Third Party
from omni.isaac.core.utils.extensions import enable_extension
# CuRobo
from curobo.util_file import get_assets_path, get_filename, get_path_of_dir, join_path
def add_extensions(simulation_app, headless_mode: Optional[str] = None):
ext_list = [
"omni.kit.asset_converter",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
]
if headless_mode is not None:
log_warn("Running in headless mode: " + headless_mode)
ext_list += ["omni.kit.livestream." + headless_mode]
[enable_extension(x) for x in ext_list]
simulation_app.update()
return True
############################################################
def add_robot_to_scene(
robot_config: Dict,
@@ -72,14 +100,18 @@ def add_robot_to_scene(
# print(prim_path)
# robot_prim = my_world.scene.stage.OverridePrim(prim_path)
# robot_prim.GetReferences().AddReference(dest_path)
robot = my_world.scene.add(
Robot(
prim_path=robot_path,
name=robot_name,
position=position,
)
robot_p = Robot(
prim_path=robot_path,
name=robot_name,
position=position,
)
if ISAAC_SIM_23:
robot_p.set_solver_velocity_iteration_count(4)
robot_p.set_solver_position_iteration_count(44)
my_world._physics_context.set_solver_type("PGS")
robot = my_world.scene.add(robot_p)
return robot, robot_path

81
examples/isaac_sim/ik_reachability.py
View File

@@ -17,25 +17,12 @@ a = torch.zeros(4, device="cuda:0")
# Standard Library
import argparse
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.rollout.rollout_base import Goal
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.state import JointState
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless", action="store_true", help="When True, enables headless mode", default=False
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
parser.add_argument(
"--visualize_spheres",
@@ -54,52 +41,49 @@ from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{
"headless": args.headless,
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Third Party
from omni.isaac.core.utils.extensions import enable_extension
# CuRobo
from curobo.util_file import (
get_assets_path,
get_filename,
get_path_of_dir,
get_robot_configs_path,
join_path,
load_yaml,
)
ext_list = [
"omni.kit.asset_converter",
# "omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
"omni.isaac.urdf",
]
[enable_extension(x) for x in ext_list]
simulation_app.update()
# Standard Library
from typing import Dict
# Third Party
import carb
import numpy as np
from helper import add_robot_to_scene
from helper import add_extensions, add_robot_to_scene
from omni.isaac.core import World
from omni.isaac.core.objects import cuboid, sphere
from omni.isaac.core.robots import Robot
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.types import ArticulationAction
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.rollout.rollout_base import Goal
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.state import JointState
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import (
get_assets_path,
get_filename,
get_path_of_dir,
get_robot_configs_path,
get_world_configs_path,
join_path,
load_yaml,
)
from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
########### OV #################
############################################################
@@ -225,6 +209,7 @@ def main():
result = ik_solver.solve_batch(goal_pose)
print("Curobo is Ready")
add_extensions(simulation_app, args.headless_mode)
usd_help.load_stage(my_world.stage)
usd_help.add_world_to_stage(world_cfg, base_frame="/World")
@@ -246,7 +231,7 @@ def main():
step_index = my_world.current_time_step_index
# print(step_index)
if step_index == 0:
if step_index <= 2:
my_world.reset()
idx_list = [robot.get_dof_index(x) for x in j_names]
robot.set_joint_positions(default_config, idx_list)

61
examples/isaac_sim/load_all_robots.py
View File

@@ -22,30 +22,13 @@ import argparse
# Third Party
from omni.isaac.kit import SimulationApp
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelConfig
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import RobotConfig
from curobo.types.state import JointState
from curobo.util_file import (
get_motion_gen_robot_list,
get_robot_configs_path,
get_robot_path,
get_world_configs_path,
join_path,
load_yaml,
)
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless", action="store_true", help="When True, enables headless mode", default=False
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
parser.add_argument(
"--visualize_spheres",
@@ -57,38 +40,31 @@ parser.add_argument(
args = parser.parse_args()
simulation_app = SimulationApp(
{
"headless": args.headless,
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Third Party
from omni.isaac.core.utils.extensions import enable_extension
ext_list = [
"omni.kit.asset_converter",
# "omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
]
[enable_extension(x) for x in ext_list]
# Third Party
import carb
import numpy as np
from helper import add_robot_to_scene
from helper import add_extensions, add_robot_to_scene
from omni.isaac.core import World
from omni.isaac.core.materials import OmniPBR
from omni.isaac.core.objects import cuboid, sphere
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core.objects import sphere
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import RobotConfig
########### OV #################
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import get_motion_gen_robot_list, get_robot_configs_path, join_path, load_yaml
def main():
@@ -153,6 +129,7 @@ def main():
setup_curobo_logger("warn")
add_extensions(simulation_app, args.headless_mode)
while simulation_app.is_running():
my_world.step(render=True)
@@ -163,7 +140,7 @@ def main():
continue
step_index = my_world.current_time_step_index
if step_index == 0:
if step_index <= 2:
my_world.reset()
for ri, robot in enumerate(robot_list):
j_names = robot_cfg_list[ri]["kinematics"]["cspace"]["joint_names"]

71
examples/isaac_sim/motion_gen_reacher.py
View File

@@ -18,23 +18,15 @@ a = torch.zeros(4, device="cuda:0")
# Standard Library
import argparse
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.state import JointState
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless", action="store_true", help="When True, enables headless mode", default=False
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
parser.add_argument(
"--visualize_spheres",
action="store_true",
@@ -42,7 +34,6 @@ parser.add_argument(
default=False,
)
parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
args = parser.parse_args()
############################################################
@@ -52,51 +43,44 @@ from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{
"headless": args.headless,
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Third Party
from omni.isaac.core.utils.extensions import enable_extension
# CuRobo
from curobo.util_file import (
get_assets_path,
get_filename,
get_path_of_dir,
get_robot_configs_path,
join_path,
load_yaml,
)
ext_list = [
"omni.kit.asset_converter",
# "omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
]
[enable_extension(x) for x in ext_list]
# simulation_app.update()
# Standard Library
from typing import Dict
# Third Party
import carb
import numpy as np
from helper import add_robot_to_scene
from helper import add_extensions, add_robot_to_scene
from omni.isaac.core import World
from omni.isaac.core.objects import cuboid, sphere
from omni.isaac.core.robots import Robot
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.types import ArticulationAction
# CuRobo
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.types.state import JointState
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import (
get_assets_path,
get_filename,
get_path_of_dir,
get_robot_configs_path,
get_world_configs_path,
join_path,
load_yaml,
)
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
############################################################
@@ -105,6 +89,8 @@ from curobo.util.usd_helper import UsdHelper
def main():
# create a curobo motion gen instance:
# assuming obstacles are in objects_path:
my_world = World(stage_units_in_meters=1.0)
stage = my_world.stage
@@ -181,6 +167,9 @@ def main():
motion_gen.warmup(enable_graph=False, warmup_js_trajopt=False)
print("Curobo is Ready")
add_extensions(simulation_app, args.headless_mode)
plan_config = MotionGenPlanConfig(
enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
)

47
examples/isaac_sim/motion_gen_reacher_nvblox.py
View File

@@ -18,21 +18,13 @@ a = torch.zeros(4, device="cuda:0")
# Standard Library
import argparse
# CuRobo
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.state import JointState
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless", action="store_true", help="When True, enables headless mode", default=False
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
parser.add_argument(
"--visualize_spheres",
@@ -51,23 +43,21 @@ from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{
"headless": args.headless,
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Third Party
from omni.isaac.core.utils.extensions import enable_extension
# CuRobo
from curobo.util_file import (
get_assets_path,
get_filename,
get_path_of_dir,
get_robot_configs_path,
join_path,
load_yaml,
)
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.types.state import JointState
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
ext_list = [
"omni.kit.asset_converter",
@@ -78,19 +68,15 @@ ext_list = [
# [enable_extension(x) for x in ext_list]
# simulation_app.update()
# Standard Library
from typing import Dict
# Third Party
import carb
import numpy as np
from helper import add_robot_to_scene
from helper import add_extensions, add_robot_to_scene
from omni.isaac.core import World
from omni.isaac.core.objects import cuboid, sphere
from omni.isaac.core.robots import Robot
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.types import ArticulationAction
# CuRobo
@@ -177,6 +163,7 @@ def main():
motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
print("Curobo is Ready")
add_extensions(simulation_app, args.headless_mode)
plan_config = MotionGenPlanConfig(
enable_graph=False, enable_graph_attempt=4, max_attempts=2, enable_finetune_trajopt=True
)

61
examples/isaac_sim/mpc_example.py
View File

@@ -24,15 +24,16 @@ a = torch.zeros(4, device="cuda:0")
# Standard Library
import argparse
import pickle
import shutil
import sys
## import curobo:
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless", action="store_true", help="When True, enables headless mode", default=False
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
parser.add_argument(
"--visualize_spheres",
@@ -50,20 +51,15 @@ args = parser.parse_args()
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{"headless": False, "width": 1920, "height": 1080}
) # _make_simulation_app({"headless": False})
{
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Third Party
# Enable the layers and stage windows in the UI
from omni.isaac.core.utils.extensions import enable_extension
ext_list = [
"omni.kit.asset_converter",
# "omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
]
[enable_extension(x) for x in ext_list]
simulation_app.update()
# Standard Library
import os
@@ -72,24 +68,8 @@ import carb
import numpy as np
from helper import add_robot_to_scene
from omni.isaac.core import World
from omni.isaac.core.objects import DynamicCuboid, VisualCuboid, cuboid, sphere
from omni.isaac.core.prims.xform_prim import XFormPrim
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
# from omni.isaac.cortex.motion_commander import MotionCommand, PosePq, open_gripper, close_gripper
from omni.isaac.core.utils.rotations import euler_angles_to_quat
########### frame prim #################
from omni.isaac.core.utils.stage import get_stage_units
from omni.isaac.core.objects import cuboid
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.cortex.cortex_object import CortexObject
from omni.isaac.franka import KinematicsSolver
from omni.isaac.franka.franka import Franka
from omni.isaac.franka.tasks import FollowTarget
from pxr import Gf
from scipy.spatial.transform import Rotation as R
# CuRobo
from curobo.util.logger import setup_curobo_logger
@@ -111,23 +91,16 @@ DATA_DIR = os.path.join(EXT_DIR, "data")
from typing import Optional
# Third Party
from helper import add_robot_to_scene
from omni.isaac.core.materials import PhysicsMaterial, VisualMaterial
from omni.isaac.core.utils.prims import add_reference_to_stage, get_prim_at_path
from pxr import Sdf, UsdShade
from helper import add_extensions, add_robot_to_scene
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
from curobo.curobolib import geom_cu
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.rollout.cost.self_collision_cost import SelfCollisionCost, SelfCollisionCostConfig
from curobo.rollout.rollout_base import Goal
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.robot import JointState
from curobo.types.state import JointState
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
@@ -271,11 +244,13 @@ def main():
goal_buffer = mpc.setup_solve_single(goal, 1)
mpc.update_goal(goal_buffer)
mpc_result = mpc.step(current_state, max_attempts=2)
usd_help.load_stage(my_world.stage)
init_world = False
cmd_state_full = None
step = 0
add_extensions(simulation_app, args.headless_mode)
while simulation_app.is_running():
if not init_world:
for _ in range(10):
@@ -289,7 +264,7 @@ def main():
step_index = my_world.current_time_step_index
if step_index == 0:
if step_index <= 2:
my_world.reset()
idx_list = [robot.get_dof_index(x) for x in j_names]
robot.set_joint_positions(default_config, idx_list)

100
examples/isaac_sim/mpc_nvblox_example.py
View File

@@ -24,15 +24,16 @@ a = torch.zeros(4, device="cuda:0")
# Standard Library
import argparse
import pickle
import shutil
import sys
## import curobo:
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless", action="store_true", help="When True, enables headless mode", default=False
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
parser.add_argument(
"--visualize_spheres",
@@ -50,50 +51,41 @@ args = parser.parse_args()
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{"headless": False, "width": 1920, "height": 1080}
) # _make_simulation_app({"headless": False})
# Third Party
# Enable the layers and stage windows in the UI
from omni.isaac.core.utils.extensions import enable_extension
ext_list = [
"omni.kit.asset_converter",
# "omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
]
[enable_extension(x) for x in ext_list]
simulation_app.update()
{
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Standard Library
import os
# Third Party
import carb
import numpy as np
from helper import add_robot_to_scene
from helper import add_extensions, add_robot_to_scene
from omni.isaac.core import World
from omni.isaac.core.objects import DynamicCuboid, VisualCuboid, cuboid, sphere
from omni.isaac.core.prims.xform_prim import XFormPrim
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
# from omni.isaac.cortex.motion_commander import MotionCommand, PosePq, open_gripper, close_gripper
from omni.isaac.core.utils.rotations import euler_angles_to_quat
from omni.isaac.core.objects import cuboid
########### frame prim #################
from omni.isaac.core.utils.stage import get_stage_units
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.cortex.cortex_object import CortexObject
from omni.isaac.franka import KinematicsSolver
from omni.isaac.franka.franka import Franka
from omni.isaac.franka.tasks import FollowTarget
from pxr import Gf
from scipy.spatial.transform import Rotation as R
# CuRobo
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.rollout.rollout_base import Goal
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.types.state import JointState
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
########### OV #################
############################################################
@@ -101,37 +93,6 @@ from curobo.util.usd_helper import UsdHelper
########### OV #################;;;;;
###########
EXT_DIR = os.path.abspath(os.path.join(os.path.abspath(os.path.dirname(__file__))))
DATA_DIR = os.path.join(EXT_DIR, "data")
########### frame prim #################;;;;;
# Standard Library
from typing import Optional
# Third Party
from helper import add_robot_to_scene
from omni.isaac.core.materials import PhysicsMaterial, VisualMaterial
from omni.isaac.core.utils.prims import add_reference_to_stage, get_prim_at_path
from pxr import Sdf, UsdShade
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
from curobo.curobolib import geom_cu
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.rollout.cost.self_collision_cost import SelfCollisionCost, SelfCollisionCostConfig
from curobo.rollout.rollout_base import Goal
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.state import JointState
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
############################################################
@@ -187,12 +148,9 @@ def main():
setup_curobo_logger("warn")
past_pose = None
n_obstacle_cuboids = 30
n_obstacle_mesh = 10
# warmup curobo instance
usd_help = UsdHelper()
target_pose = None
tensor_args = TensorDeviceType()
@@ -202,7 +160,7 @@ def main():
default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
robot_cfg["kinematics"]["collision_sphere_buffer"] += 0.02
robot, robot_prim_path = add_robot_to_scene(robot_cfg, my_world)
robot, _ = add_robot_to_scene(robot_cfg, my_world)
articulation_controller = robot.get_articulation_controller()
@@ -260,12 +218,16 @@ def main():
goal_buffer = mpc.setup_solve_single(goal, 1)
mpc.update_goal(goal_buffer)
mpc_result = mpc.step(current_state, max_attempts=2)
add_extensions(simulation_app, args.headless_mode)
usd_help.load_stage(my_world.stage)
usd_help.add_world_to_stage(world_cfg.get_mesh_world(), base_frame="/World")
init_world = False
cmd_state_full = None
step = 0
add_extensions(simulation_app, args.headless_mode)
while simulation_app.is_running():
if not init_world:
for _ in range(10):

66
examples/isaac_sim/multi_arm_reacher.py
View File

@@ -18,23 +18,13 @@ a = torch.zeros(4, device="cuda:0")
# Standard Library
import argparse
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.rollout.rollout_base import Goal
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.state import JointState
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless", action="store_true", help="When True, enables headless mode", default=False
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
parser.add_argument(
"--visualize_spheres",
@@ -55,51 +45,36 @@ from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{
"headless": args.headless,
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Third Party
from omni.isaac.core.utils.extensions import enable_extension
# CuRobo
from curobo.util_file import (
get_assets_path,
get_filename,
get_path_of_dir,
get_robot_configs_path,
join_path,
load_yaml,
)
ext_list = [
"omni.kit.asset_converter",
"omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
]
# [enable_extension(x) for x in ext_list]
# simulation_app.update()
# Standard Library
from typing import Dict
# Third Party
import carb
import numpy as np
from helper import add_robot_to_scene
from helper import add_extensions, add_robot_to_scene
from omni.isaac.core import World
from omni.isaac.core.objects import cuboid, sphere
from omni.isaac.core.robots import Robot
########### OV #################
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.types import ArticulationAction
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
# from curobo.wrap.reacher.ik_solver import IKSolver, IKSolverConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import WorldConfig
from curobo.rollout.rollout_base import Goal
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.state import JointState
from curobo.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
############################################################
@@ -178,6 +153,7 @@ def main():
motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False)
print("Curobo is Ready")
add_extensions(simulation_app, args.headless_mode)
plan_config = MotionGenPlanConfig(
enable_graph=False, enable_graph_attempt=4, max_attempts=10, enable_finetune_trajopt=True
)
@@ -240,7 +216,7 @@ def main():
step_index = my_world.current_time_step_index
# print(step_index)
if step_index == 0:
if step_index <= 2:
my_world.reset()
idx_list = [robot.get_dof_index(x) for x in j_names]
robot.set_joint_positions(default_config, idx_list)

14
examples/isaac_sim/realsense_collision.py
View File

@@ -21,6 +21,13 @@ from matplotlib import cm
from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{
"headless": False,
"width": "1920",
"height": "1080",
}
)
# CuRobo
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.types import Cuboid, WorldConfig
@@ -30,13 +37,6 @@ from curobo.types.math import Pose
from curobo.util_file import get_world_configs_path, join_path, load_yaml
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
simulation_app = SimulationApp(
{
"headless": False,
"width": "1920",
"height": "1080",
}
)
simulation_app.update()
# Third Party
from omni.isaac.core import World

19
examples/isaac_sim/realsense_mpc.py
View File

@@ -13,13 +13,23 @@
import torch
a = torch.zeros(4, device="cuda:0")
# Third Party
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{
"headless": False,
"width": "1920",
"height": "1080",
}
)
# Third Party
import cv2
import numpy as np
import torch
from matplotlib import cm
from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
from omni.isaac.kit import SimulationApp
# CuRobo
from curobo.geom.sdf.world import CollisionCheckerType
@@ -33,13 +43,6 @@ from curobo.util_file import get_robot_configs_path, get_world_configs_path, joi
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
simulation_app = SimulationApp(
{
"headless": False,
"width": "1920",
"height": "1080",
}
)
simulation_app.update()
# Standard Library
import argparse

19
examples/isaac_sim/realsense_reacher.py
View File

@@ -13,13 +13,23 @@
import torch
a = torch.zeros(4, device="cuda:0")
# Third Party
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{
"headless": False,
"width": "1920",
"height": "1080",
}
)
# Third Party
import cv2
import numpy as np
import torch
from matplotlib import cm
from nvblox_torch.datasets.realsense_dataset import RealsenseDataloader
from omni.isaac.kit import SimulationApp
# CuRobo
from curobo.geom.sdf.world import CollisionCheckerType
@@ -33,13 +43,6 @@ from curobo.util_file import get_robot_configs_path, get_world_configs_path, joi
from curobo.wrap.model.robot_world import RobotWorld, RobotWorldConfig
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
simulation_app = SimulationApp(
{
"headless": False,
"width": "1920",
"height": "1080",
}
)
simulation_app.update()
# Standard Library
import argparse

127
examples/isaac_sim/simple_stacking.py
View File

@@ -20,21 +20,41 @@ import numpy as np
np.set_printoptions(suppress=True)
# Standard Library
import os.path as osp
# Standard Library
import argparse
## import curobo:
parser = argparse.ArgumentParser()
parser.add_argument(
"--headless_mode",
type=str,
default=None,
help="To run headless, use one of [native, websocket], webrtc might not work.",
)
args = parser.parse_args()
# Third Party
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False}) # This adds paths for the following importing
simulation_app = SimulationApp(
{
"headless": args.headless_mode is not None,
"width": "1920",
"height": "1080",
}
)
# Standard Library
from typing import Optional
# Third Party
import carb
from helper import add_extensions
from omni.isaac.core import World
from omni.isaac.core.controllers import BaseController
from omni.isaac.core.tasks import Stacking as BaseStacking
from omni.isaac.core.utils.extensions import enable_extension
from omni.isaac.core.utils.prims import is_prim_path_valid
from omni.isaac.core.utils.stage import get_stage_units
from omni.isaac.core.utils.string import find_unique_string_name
@@ -43,14 +63,13 @@ from omni.isaac.core.utils.viewports import set_camera_view
from omni.isaac.franka import Franka
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.geom.sphere_fit import SphereFitType
from curobo.geom.types import WorldConfig
from curobo.rollout.rollout_base import Goal
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState, RobotConfig
from curobo.types.robot import JointState
from curobo.types.state import JointState
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import get_robot_configs_path, get_world_configs_path, join_path, load_yaml
@@ -60,18 +79,6 @@ from curobo.wrap.reacher.motion_gen import (
MotionGenPlanConfig,
MotionGenResult,
)
from curobo.wrap.reacher.mpc import MpcSolver, MpcSolverConfig
ext_list = [
"omni.kit.asset_converter",
"omni.kit.livestream.native",
"omni.kit.tool.asset_importer",
"omni.isaac.asset_browser",
# "omni.kit.window.sequencer.scripts.sequencer_extension",
"omni.kit.window.movie_capture",
]
[enable_extension(x) for x in ext_list] # toggle this for remote streaming
simulation_app.update()
class CuroboController(BaseController):
@@ -85,6 +92,7 @@ class CuroboController(BaseController):
self._save_log = False
self.my_world = my_world
self.my_task = my_task
self._step_idx = 0
n_obstacle_cuboids = 20
n_obstacle_mesh = 2
# warmup curobo instance
@@ -111,7 +119,7 @@ class CuroboController(BaseController):
] = "panda_hand" # controls which frame the controller is controlling
# self.robot_cfg["kinematics"]["cspace"]["max_acceleration"] = 10.0 # controls how fast robot moves
self.robot_cfg["kinematics"]["extra_collision_spheres"] = {"attached_object": 100}
self.robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.0
# @self.robot_cfg["kinematics"]["collision_sphere_buffer"] = 0.0
self.robot_cfg["kinematics"]["collision_spheres"] = "spheres/franka_collision_mesh.yml"
world_cfg_table = WorldConfig.from_dict(
@@ -133,16 +141,13 @@ class CuroboController(BaseController):
trajopt_tsteps=32,
collision_checker_type=CollisionCheckerType.MESH,
use_cuda_graph=True,
num_ik_seeds=40,
num_trajopt_seeds=10,
num_graph_seeds=10,
interpolation_dt=0.01,
collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
store_ik_debug=self._save_log,
store_trajopt_debug=self._save_log,
state_finite_difference_mode="CENTRAL",
minimize_jerk=True,
acceleration_scale=0.3,
acceleration_scale=0.5,
fixed_iters_trajopt=True,
)
self.motion_gen = MotionGen(motion_gen_config)
@@ -159,6 +164,7 @@ class CuroboController(BaseController):
self.usd_help.load_stage(self.my_world.stage)
self.cmd_plan = None
self.cmd_idx = 0
self._step_idx = 0
self.idx_list = None
def attach_obj(
@@ -233,6 +239,7 @@ class CuroboController(BaseController):
if self.cmd_plan is None:
self.cmd_idx = 0
self._step_idx = 0
# Set EE goals
ee_translation_goal = self.my_task.target_position
ee_orientation_goal = np.array([0, 0, -1, 0])
@@ -246,19 +253,22 @@ class CuroboController(BaseController):
else:
carb.log_warn("Plan did not converge to a solution.")
return None
if self._step_idx % 3 == 0:
cmd_state = self.cmd_plan[self.cmd_idx]
self.cmd_idx += 1
cmd_state = self.cmd_plan[self.cmd_idx]
self.cmd_idx += 1
# get full dof state
art_action = ArticulationAction(
cmd_state.position.cpu().numpy(),
# cmd_state.velocity.cpu().numpy(),
joint_indices=self.idx_list,
)
if self.cmd_idx >= len(self.cmd_plan.position):
self.cmd_idx = 0
self.cmd_plan = None
# get full dof state
art_action = ArticulationAction(
cmd_state.position.cpu().numpy(),
# cmd_state.velocity.cpu().numpy(),
joint_indices=self.idx_list,
)
if self.cmd_idx >= len(self.cmd_plan.position):
self.cmd_idx = 0
self.cmd_plan = None
else:
art_action = None
self._step_idx += 1
return art_action
def reached_target(self, observations: dict) -> bool:
@@ -413,9 +423,39 @@ articulation_controller = my_franka.get_articulation_controller()
set_camera_view(eye=[2, 0, 1], target=[0.00, 0.00, 0.00], camera_prim_path="/OmniverseKit_Persp")
wait_steps = 8
initial_steps = 10
my_franka.set_solver_velocity_iteration_count(4)
my_franka.set_solver_position_iteration_count(124)
my_world._physics_context.set_solver_type("TGS")
initial_steps = 100
################################################################
print("Start simulation...")
robot = my_franka
print(
my_world._physics_context.get_solver_type(),
robot.get_solver_position_iteration_count(),
robot.get_solver_velocity_iteration_count(),
)
print(my_world._physics_context.use_gpu_pipeline)
print(articulation_controller.get_gains())
print(articulation_controller.get_max_efforts())
robot = my_franka
print("**********************")
robot.enable_gravity()
articulation_controller.set_gains(
kps=np.array(
[100000000, 6000000.0, 10000000, 600000.0, 25000.0, 15000.0, 50000.0, 6000.0, 6000.0]
)
)
articulation_controller.set_max_efforts(
values=np.array([100000, 52.199997, 100000, 52.199997, 7.2, 7.2, 7.2, 50.0, 50])
)
print("Updated gains:")
print(articulation_controller.get_gains())
print(articulation_controller.get_max_efforts())
# exit()
my_franka.gripper.open()
for _ in range(wait_steps):
my_world.step(render=True)
@@ -423,18 +463,11 @@ my_task.reset()
task_finished = False
observations = my_world.get_observations()
my_task.get_pick_position(observations)
robot = my_franka
print("**********************")
# print(robot.get_solver_position_iteration_count(), robot.get_solver_velocity_iteration_count())
robot.enable_gravity()
robot._articulation_view.set_gains(
kps=np.array([100000000, 10000000]), joint_indices=np.array([0, 2])
)
robot._articulation_view.set_max_efforts(
values=np.array([10000, 10000]), joint_indices=np.array([0, 2])
)
i = 0
add_extensions(simulation_app, args.headless_mode)
while simulation_app.is_running():
my_world.step(render=True) # necessary to visualize changes
i += 1
@@ -481,7 +514,7 @@ while simulation_app.is_running():
art_action = my_controller.forward(sim_js, my_franka.dof_names)
if art_action is not None:
articulation_controller.apply_action(art_action)
for _ in range(2):
my_world.step(render=False)
# for _ in range(2):
# my_world.step(render=False)
simulation_app.close()