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

Improved precision, quality and js planner.

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-04-11 13:19:01 -07:00
parent 774dcfd609
commit d6e600c88c
51 changed files with 2128 additions and 1054 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/isaac_sim/helper.py
View File

@@ -23,6 +23,7 @@ from pxr import UsdPhysics
# CuRobo
from curobo.util.logger import log_warn
from curobo.util.usd_helper import set_prim_transform
ISAAC_SIM_23 = False
try:
@@ -107,8 +108,13 @@ def add_robot_to_scene(
robot_p = Robot(
prim_path=robot_path + "/" + base_link_name,
name=robot_name,
position=position,
)
robot_prim = robot_p.prim
stage = robot_prim.GetStage()
linkp = stage.GetPrimAtPath(robot_path)
set_prim_transform(linkp, [position[0], position[1], position[2], 1, 0, 0, 0])
if False and ISAAC_SIM_23: # this doesn't work in isaac sim 2023.1.1
robot_p.set_solver_velocity_iteration_count(0)
robot_p.set_solver_position_iteration_count(44)
@@ -116,8 +122,6 @@ def add_robot_to_scene(
my_world._physics_context.set_solver_type("PGS")
if ISAAC_SIM_23: # fix to load robot correctly in isaac sim 2023.1.1
robot_prim = robot_p.prim
stage = robot_prim.GetStage()
linkp = stage.GetPrimAtPath(robot_path + "/" + base_link_name)
mass = UsdPhysics.MassAPI(linkp)
mass.GetMassAttr().Set(0)

5
examples/isaac_sim/motion_gen_reacher.py
View File

@@ -205,7 +205,7 @@ def main():
interpolation_dt = 0.05
if args.reactive:
trajopt_tsteps = 40
trajopt_dt = 0.05
trajopt_dt = 0.04
optimize_dt = False
max_attempts = 1
trim_steps = [1, None]
@@ -223,9 +223,6 @@ def main():
trajopt_dt=trajopt_dt,
trajopt_tsteps=trajopt_tsteps,
trim_steps=trim_steps,
# velocity_scale=0.1,
# self_collision_check=False,
# self_collision_opt=False,
)
motion_gen = MotionGen(motion_gen_config)
print("warming up...")

6
examples/isaac_sim/multi_arm_reacher.py
View File

@@ -136,7 +136,6 @@ def main():
robot_cfg,
world_cfg,
tensor_args,
trajopt_tsteps=40,
collision_checker_type=CollisionCheckerType.MESH,
use_cuda_graph=True,
num_trajopt_seeds=12,
@@ -146,6 +145,7 @@ def main():
collision_activation_distance=0.025,
acceleration_scale=1.0,
fixed_iters_trajopt=True,
trajopt_tsteps=40,
)
motion_gen = MotionGen(motion_gen_config)
print("warming up...")
@@ -154,7 +154,9 @@ def main():
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
enable_graph=False,
enable_graph_attempt=4,
max_attempts=10,
)
usd_help.load_stage(my_world.stage)

2
examples/isaac_sim/simple_stacking.py
View File

@@ -162,7 +162,7 @@ class CuroboController(BaseController):
pose_metric = None
if constrain_grasp_approach:
pose_metric = PoseCostMetric.create_grasp_approach_metric(
offset_position=0.1, tstep_fraction=0.6
offset_position=0.1, tstep_fraction=0.8
)
self.plan_config = MotionGenPlanConfig(

2
examples/motion_gen_example.py
View File

@@ -107,7 +107,7 @@ def demo_motion_gen_mesh():
motion_gen = MotionGen(motion_gen_config)
robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
retract_cfg = robot_cfg.retract_config
retract_cfg = robot_cfg.cpsace.retract_config
state = motion_gen.rollout_fn.compute_kinematics(
JointState.from_position(retract_cfg.view(1, -1))
)

2
examples/motion_gen_profile.py
View File

@@ -55,7 +55,7 @@ def demo_motion_gen():
motion_gen = MotionGen(motion_gen_config)
robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
robot_cfg = RobotConfig.from_dict(robot_cfg, tensor_args)
retract_cfg = robot_cfg.retract_config
retract_cfg = robot_cfg.cspace.retract_config
state = motion_gen.rollout_fn.compute_kinematics(
JointState.from_position(retract_cfg.view(1, -1))
)

17
examples/usd_example.py
View File

@@ -51,14 +51,13 @@ def get_trajectory(robot_file="franka.yml", dt=1.0 / 60.0):
robot_file,
world_file,
tensor_args,
trajopt_tsteps=24,
trajopt_tsteps=32,
collision_checker_type=CollisionCheckerType.PRIMITIVE,
use_cuda_graph=True,
num_trajopt_seeds=2,
num_graph_seeds=2,
evaluate_interpolated_trajectory=True,
interpolation_dt=dt,
self_collision_check=True,
)
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup()
@@ -70,13 +69,15 @@ def get_trajectory(robot_file="franka.yml", dt=1.0 / 60.0):
)
retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
start_state = JointState.from_position(retract_cfg.view(1, -1).clone() + 0.5)
start_state = JointState.from_position(retract_cfg.view(1, -1).clone())
start_state.position[..., :-2] += 0.5
result = motion_gen.plan_single(start_state, retract_pose)
traj = result.get_interpolated_plan() # optimized plan
return traj
def save_curobo_robot_world_to_usd(robot_file="franka.yml"):
print(robot_file)
tensor_args = TensorDeviceType()
world_file = "collision_test.yml"
world_model = WorldConfig.from_dict(
@@ -87,16 +88,18 @@ def save_curobo_robot_world_to_usd(robot_file="franka.yml"):
q_traj = get_trajectory(robot_file, dt)
if q_traj is not None:
q_start = q_traj[0]
UsdHelper.write_trajectory_animation_with_robot_usd(
robot_file,
world_model,
q_start,
q_traj,
save_path="test.usda",
save_path="test.usd",
robot_color=[0.5, 0.5, 0.2, 1.0],
base_frame="/grid_world_1",
flatten_usd=True,
)
else:
print("failed")
def save_curobo_robot_to_usd(robot_file="franka.yml"):
@@ -241,5 +244,5 @@ def save_log_motion_gen(robot_file: str = "franka.yml"):
if __name__ == "__main__":
# save_curobo_world_to_usd()
setup_curobo_logger("error")
save_log_motion_gen("franka.yml")
# save_curobo_robot_world_to_usd("franka.yml")
# save_log_motion_gen("franka.yml")
save_curobo_robot_world_to_usd("ur5e_robotiq_2f_140.yml")