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

update to 0.6.2

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2023-12-15 02:01:33 -08:00
parent d85ae41fba
commit 58958bbcce
105 changed files with 2514 additions and 934 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
examples/isaac_sim/batch_motion_gen_reacher.py
View File

@@ -38,6 +38,8 @@ parser.add_argument(
parser.add_argument("--robot", type=str, default="franka.yml", help="robot configuration to load")
args = parser.parse_args()
# Third Party
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp(
{
@@ -150,13 +152,9 @@ def main():
interpolation_dt=0.03,
collision_cache={"obb": 6, "mesh": 6},
collision_activation_distance=0.025,
acceleration_scale=1.0,
self_collision_check=True,
maximum_trajectory_dt=0.25,
fixed_iters_trajopt=True,
finetune_dt_scale=1.05,
grad_trajopt_iters=300,
minimize_jerk=False,
)
motion_gen = MotionGen(motion_gen_config)
j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
@@ -194,7 +192,7 @@ def main():
continue
step_index = my_world.current_time_step_index
if step_index == 0:
if step_index <= 2:
my_world.reset()
for robot in robot_list:
idx_list = [robot.get_dof_index(x) for x in j_names]
@@ -246,7 +244,7 @@ def main():
if (
(torch.sum(prev_distance[0] > 1e-2) or torch.sum(prev_distance[1] > 1e-2))
and (torch.sum(past_distance[0]) == 0.0 and torch.sum(past_distance[1] == 0.0))
and torch.norm(full_js.velocity) < 0.01
and torch.max(torch.abs(full_js.velocity)) < 0.2
and cmd_plan[0] is None
and cmd_plan[1] is None
):
@@ -254,23 +252,24 @@ def main():
result = motion_gen.plan_batch_env(full_js, ik_goal, plan_config.clone())
prev_goal.copy_(ik_goal)
trajs = result.get_paths()
for s in range(len(result.success)):
if result.success[s]:
cmd_plan[s] = motion_gen.get_full_js(trajs[s])
# cmd_plan = result.get_interpolated_plan()
# cmd_plan = motion_gen.get_full_js(cmd_plan)
# get only joint names that are in both:
idx_list = []
common_js_names = []
for x in sim_js_names:
if x in cmd_plan[s].joint_names:
idx_list.append(robot_list[s].get_dof_index(x))
common_js_names.append(x)
if torch.count_nonzero(result.success) > 0:
trajs = result.get_paths()
for s in range(len(result.success)):
if result.success[s]:
cmd_plan[s] = motion_gen.get_full_js(trajs[s])
# cmd_plan = result.get_interpolated_plan()
# cmd_plan = motion_gen.get_full_js(cmd_plan)
# get only joint names that are in both:
idx_list = []
common_js_names = []
for x in sim_js_names:
if x in cmd_plan[s].joint_names:
idx_list.append(robot_list[s].get_dof_index(x))
common_js_names.append(x)
cmd_plan[s] = cmd_plan[s].get_ordered_joint_state(common_js_names)
cmd_plan[s] = cmd_plan[s].get_ordered_joint_state(common_js_names)
cmd_idx = 0
cmd_idx = 0
# print(cmd_plan)
for s in range(len(cmd_plan)):

8
examples/isaac_sim/helper.py
View File

@@ -82,7 +82,13 @@ def add_robot_to_scene(
import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION
import_config.distance_scale = 1
import_config.density = 0.0
full_path = join_path(get_assets_path(), robot_config["kinematics"]["urdf_path"])
asset_path = get_assets_path()
if (
"external_asset_path" in robot_config["kinematics"]
and robot_config["kinematics"]["external_asset_path"] is not None
):
asset_path = robot_config["kinematics"]["external_asset_path"]
full_path = join_path(asset_path, robot_config["kinematics"]["urdf_path"])
robot_path = get_path_of_dir(full_path)
filename = get_filename(full_path)
imported_robot = urdf_interface.parse_urdf(robot_path, filename, import_config)

97
examples/isaac_sim/motion_gen_reacher.py
View File

@@ -26,6 +26,18 @@ parser.add_argument(
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(
"--external_asset_path",
type=str,
default=None,
help="Path to external assets when loading an externally located robot",
)
parser.add_argument(
"--external_robot_configs_path",
type=str,
default=None,
help="Path to external robot config when loading an external robot",
)
parser.add_argument(
"--visualize_spheres",
@@ -33,7 +45,12 @@ parser.add_argument(
help="When True, visualizes robot spheres",
default=False,
)
parser.add_argument(
"--reactive",
action="store_true",
help="When True, runs in reactive mode",
default=False,
)
args = parser.parse_args()
############################################################
@@ -69,7 +86,7 @@ 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.logger import log_error, setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import (
get_assets_path,
@@ -114,16 +131,22 @@ def main():
setup_curobo_logger("warn")
past_pose = None
n_obstacle_cuboids = 30
n_obstacle_mesh = 10
n_obstacle_mesh = 100
# warmup curobo instance
usd_help = UsdHelper()
target_pose = None
tensor_args = TensorDeviceType()
robot_cfg_path = get_robot_configs_path()
if args.external_robot_configs_path is not None:
robot_cfg_path = args.external_robot_configs_path
robot_cfg = load_yaml(join_path(robot_cfg_path, args.robot))["robot_cfg"]
robot_cfg = load_yaml(join_path(get_robot_configs_path(), args.robot))["robot_cfg"]
if args.external_asset_path is not None:
robot_cfg["kinematics"]["external_asset_path"] = args.external_asset_path
if args.external_robot_configs_path is not None:
robot_cfg["kinematics"]["external_robot_configs_path"] = args.external_robot_configs_path
j_names = robot_cfg["kinematics"]["cspace"]["joint_names"]
default_config = robot_cfg["kinematics"]["cspace"]["retract_config"]
@@ -143,35 +166,45 @@ def main():
world_cfg = WorldConfig(cuboid=world_cfg_table.cuboid, mesh=world_cfg1.mesh)
trajopt_dt = None
optimize_dt = True
trajopt_tsteps = 32
trim_steps = None
max_attempts = 4
if args.reactive:
trajopt_tsteps = 36
trajopt_dt = 0.05
optimize_dt = False
max_attemtps = 1
trim_steps = [1, None]
motion_gen_config = MotionGenConfig.load_from_robot_config(
robot_cfg,
world_cfg,
tensor_args,
trajopt_tsteps=32,
collision_checker_type=CollisionCheckerType.MESH,
use_cuda_graph=True,
num_trajopt_seeds=12,
num_graph_seeds=12,
interpolation_dt=0.03,
interpolation_dt=0.05,
collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
collision_activation_distance=0.025,
acceleration_scale=1.0,
self_collision_check=True,
maximum_trajectory_dt=0.25,
fixed_iters_trajopt=True,
finetune_dt_scale=1.05,
velocity_scale=[0.25, 1, 1, 1, 1.0, 1.0, 1.0, 1.0, 1.0],
optimize_dt=optimize_dt,
trajopt_dt=trajopt_dt,
trajopt_tsteps=trajopt_tsteps,
trim_steps=trim_steps,
)
motion_gen = MotionGen(motion_gen_config)
print("warming up...")
motion_gen.warmup(enable_graph=False, warmup_js_trajopt=False)
motion_gen.warmup(enable_graph=True, warmup_js_trajopt=False, parallel_finetune=True)
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
enable_graph=False,
enable_graph_attempt=2,
max_attempts=max_attempts,
enable_finetune_trajopt=True,
parallel_finetune=True,
)
usd_help.load_stage(my_world.stage)
@@ -182,7 +215,7 @@ def main():
my_world.scene.add_default_ground_plane()
i = 0
spheres = None
past_cmd = None
while simulation_app.is_running():
my_world.step(render=True)
if not my_world.is_playing():
@@ -219,6 +252,7 @@ def main():
"/curobo",
],
).get_collision_check_world()
print(len(obstacles.objects))
motion_gen.update_world(obstacles)
print("Updated World")
@@ -234,13 +268,24 @@ def main():
sim_js = robot.get_joints_state()
sim_js_names = robot.dof_names
if np.any(np.isnan(sim_js.positions)):
log_error("isaac sim has returned NAN joint position values.")
cu_js = JointState(
position=tensor_args.to_device(sim_js.positions),
velocity=tensor_args.to_device(sim_js.velocities) * 0.0,
velocity=tensor_args.to_device(sim_js.velocities), # * 0.0,
acceleration=tensor_args.to_device(sim_js.velocities) * 0.0,
jerk=tensor_args.to_device(sim_js.velocities) * 0.0,
joint_names=sim_js_names,
)
if not args.reactive:
cu_js.velocity *= 0.0
cu_js.acceleration *= 0.0
if args.reactive and past_cmd is not None:
cu_js.position[:] = past_cmd.position
cu_js.velocity[:] = past_cmd.velocity
cu_js.acceleration[:] = past_cmd.acceleration
cu_js = cu_js.get_ordered_joint_state(motion_gen.kinematics.joint_names)
if args.visualize_spheres and step_index % 2 == 0:
@@ -260,12 +305,17 @@ def main():
spheres.append(sp)
else:
for si, s in enumerate(sph_list[0]):
spheres[si].set_world_pose(position=np.ravel(s.position))
spheres[si].set_radius(float(s.radius))
if not np.isnan(s.position[0]):
spheres[si].set_world_pose(position=np.ravel(s.position))
spheres[si].set_radius(float(s.radius))
robot_static = False
if (np.max(np.abs(sim_js.velocities)) < 0.2) or args.reactive:
robot_static = True
if (
np.linalg.norm(cube_position - target_pose) > 1e-3
and np.linalg.norm(past_pose - cube_position) == 0.0
and np.linalg.norm(sim_js.velocities) < 0.2
and robot_static
):
# Set EE teleop goals, use cube for simple non-vr init:
ee_translation_goal = cube_position
@@ -303,7 +353,7 @@ def main():
past_pose = cube_position
if cmd_plan is not None:
cmd_state = cmd_plan[cmd_idx]
past_cmd = cmd_state.clone()
# get full dof state
art_action = ArticulationAction(
cmd_state.position.cpu().numpy(),
@@ -318,6 +368,7 @@ def main():
if cmd_idx >= len(cmd_plan.position):
cmd_idx = 0
cmd_plan = None
past_cmd = None
simulation_app.close()

14
examples/isaac_sim/motion_gen_reacher_nvblox.py
View File

@@ -145,18 +145,12 @@ def main():
tensor_args,
trajopt_tsteps=32,
collision_checker_type=CollisionCheckerType.BLOX,
collision_activation_distance=0.005,
use_cuda_graph=True,
num_trajopt_seeds=12,
num_graph_seeds=12,
interpolation_dt=0.03,
collision_activation_distance=0.025,
acceleration_scale=1.0,
self_collision_check=True,
maximum_trajectory_dt=0.25,
finetune_dt_scale=1.05,
fixed_iters_trajopt=True,
finetune_trajopt_iters=500,
minimize_jerk=False,
# fixed_iters_trajopt=True,
)
motion_gen = MotionGen(motion_gen_config)
print("warming up...")
@@ -188,7 +182,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)
@@ -240,7 +234,7 @@ def main():
if (
np.linalg.norm(cube_position - target_pose) > 1e-3
and np.linalg.norm(past_pose - cube_position) == 0.0
and np.linalg.norm(sim_js.velocities) < 0.2
and np.max(np.abs(sim_js.velocities)) < 0.2
):
# Set EE teleop goals, use cube for simple non-vr init:
ee_translation_goal = cube_position

5
examples/isaac_sim/multi_arm_reacher.py
View File

@@ -136,7 +136,7 @@ def main():
robot_cfg,
world_cfg,
tensor_args,
trajopt_tsteps=32,
trajopt_tsteps=40,
collision_checker_type=CollisionCheckerType.MESH,
use_cuda_graph=True,
num_trajopt_seeds=12,
@@ -145,7 +145,6 @@ def main():
collision_cache={"obb": n_obstacle_cuboids, "mesh": n_obstacle_mesh},
collision_activation_distance=0.025,
acceleration_scale=1.0,
maximum_trajectory_dt=0.2,
fixed_iters_trajopt=True,
)
motion_gen = MotionGen(motion_gen_config)
@@ -286,7 +285,7 @@ def main():
if (
np.linalg.norm(cube_position - target_pose) > 1e-3
and np.linalg.norm(past_pose - cube_position) == 0.0
and np.linalg.norm(sim_js.velocities) < 0.2
and np.max(np.abs(sim_js.velocities)) < 0.2
):
# Set EE teleop goals, use cube for simple non-vr init:
ee_translation_goal = cube_position

2
examples/isaac_sim/realsense_mpc.py
View File

@@ -330,7 +330,7 @@ if __name__ == "__main__":
if cmd_step_idx == 0:
draw_rollout_points(mpc.get_visual_rollouts(), clear=not args.use_debug_draw)
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)

4
examples/isaac_sim/realsense_reacher.py
View File

@@ -189,7 +189,7 @@ if __name__ == "__main__":
"world": {
"pose": [0, 0, 0, 1, 0, 0, 0],
"integrator_type": "occupancy",
"voxel_size": 0.03,
"voxel_size": 0.02,
}
}
}
@@ -270,7 +270,7 @@ if __name__ == "__main__":
continue
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)

1
examples/isaac_sim/simple_stacking.py
View File

@@ -146,7 +146,6 @@ class CuroboController(BaseController):
store_ik_debug=self._save_log,
store_trajopt_debug=self._save_log,
state_finite_difference_mode="CENTRAL",
minimize_jerk=True,
acceleration_scale=0.5,
fixed_iters_trajopt=True,
)

56
examples/motion_gen_example.py
View File

@@ -91,7 +91,7 @@ def plot_iters_traj_3d(trajectory, d_id=1, dof=7, seed=0):
def demo_motion_gen_mesh():
PLOT = True
PLOT = False
tensor_args = TensorDeviceType()
world_file = "collision_mesh_scene.yml"
robot_file = "franka.yml"
@@ -99,7 +99,7 @@ def demo_motion_gen_mesh():
robot_file,
world_file,
tensor_args,
trajopt_tsteps=40,
# trajopt_tsteps=40,
collision_checker_type=CollisionCheckerType.MESH,
use_cuda_graph=False,
)
@@ -129,9 +129,9 @@ def demo_motion_gen_mesh():
plot_traj(traj.cpu().numpy())
def demo_motion_gen():
def demo_motion_gen(js=False):
# Standard Library
PLOT = False
PLOT = True
tensor_args = TensorDeviceType()
world_file = "collision_table.yml"
robot_file = "franka.yml"
@@ -144,7 +144,7 @@ def demo_motion_gen():
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup(enable_graph=False)
motion_gen.warmup(enable_graph=True, warmup_js_trajopt=js)
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 = motion_gen.get_retract_config()
@@ -153,11 +153,24 @@ def demo_motion_gen():
)
retract_pose = Pose(state.ee_pos_seq.squeeze(), quaternion=state.ee_quat_seq.squeeze())
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.3)
result = motion_gen.plan_single(start_state, retract_pose, MotionGenPlanConfig(max_attempts=1))
start_state = JointState.from_position(retract_cfg.view(1, -1) + 0.1)
goal_state = start_state.clone()
goal_state.position[..., 3] -= 0.1
if js:
result = motion_gen.plan_single_js(
start_state,
goal_state,
MotionGenPlanConfig(
max_attempts=1, enable_graph=False, enable_opt=True, enable_finetune_trajopt=True
),
)
else:
result = motion_gen.plan_single(
start_state, retract_pose, MotionGenPlanConfig(max_attempts=1)
)
traj = result.get_interpolated_plan()
print("Trajectory Generated: ", result.success, result.optimized_dt.item())
if PLOT:
print("Trajectory Generated: ", result.success, result.solve_time, result.status)
if PLOT and result.success.item():
plot_traj(traj, result.interpolation_dt)
# plt.save("test.png")
# plt.close()
@@ -215,10 +228,9 @@ def demo_motion_gen_batch():
robot_file,
world_file,
tensor_args,
trajopt_tsteps=30,
collision_checker_type=CollisionCheckerType.PRIMITIVE,
use_cuda_graph=True,
num_trajopt_seeds=4,
num_trajopt_seeds=12,
num_graph_seeds=1,
num_ik_seeds=30,
)
@@ -235,12 +247,13 @@ def demo_motion_gen_batch():
retract_pose = retract_pose.repeat_seeds(2)
retract_pose.position[0, 0] = -0.3
for _ in range(2):
result = motion_gen.plan_batch(
start_state.repeat_seeds(2),
retract_pose,
MotionGenPlanConfig(enable_graph=False, enable_opt=True),
)
result = motion_gen.plan_batch(
start_state.repeat_seeds(2),
retract_pose,
MotionGenPlanConfig(
max_attempts=5, enable_graph=False, enable_graph_attempt=1, enable_opt=True
),
)
traj = result.optimized_plan.position.view(2, -1, 7) # optimized plan
print("Trajectory Generated: ", result.success)
if PLOT:
@@ -360,7 +373,8 @@ def demo_motion_gen_batch_env(n_envs: int = 10):
if __name__ == "__main__":
setup_curobo_logger("error")
demo_motion_gen()
n = [2, 10]
for n_envs in n:
demo_motion_gen_batch_env(n_envs=n_envs)
# demo_motion_gen(js=True)
demo_motion_gen_batch()
# n = [2, 10]
# for n_envs in n:
# demo_motion_gen_batch_env(n_envs=n_envs)

79
examples/pose_sequence_example.py Normal file
View File

@@ -0,0 +1,79 @@
#
# Copyright (c) 2023 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
#
# NVIDIA CORPORATION, its affiliates and licensors retain all intellectual
# property and proprietary rights in and to this material, related
# documentation and any modifications thereto. Any use, reproduction,
# disclosure or distribution of this material and related documentation
# without an express license agreement from NVIDIA CORPORATION or
# its affiliates is strictly prohibited.
#
" This example moves the robot through a sequence of poses and dumps an animated usd."
# CuRobo
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, MotionGenPlanConfig
def pose_sequence_ur5e():
# load ur5e motion gen:
world_file = "collision_table.yml"
robot_file = "ur5e.yml"
motion_gen_config = MotionGenConfig.load_from_robot_config(
robot_file,
world_file,
interpolation_dt=(1 / 30),
)
motion_gen = MotionGen(motion_gen_config)
motion_gen.warmup(parallel_finetune=True)
retract_cfg = motion_gen.get_retract_config()
start_state = JointState.from_position(retract_cfg.view(1, -1))
# poses for ur5e:
home_pose = [-0.431, 0.172, 0.348, 0, 1, 0, 0]
pose_1 = [0.157, -0.443, 0.427, 0, 1, 0, 0]
pose_2 = [0.126, -0.443, 0.729, 0, 0, 1, 0]
pose_3 = [-0.449, 0.339, 0.414, -0.681, -0.000, 0.000, 0.732]
pose_4 = [-0.449, 0.339, 0.414, 0.288, 0.651, -0.626, -0.320]
pose_5 = [-0.218, 0.508, 0.670, 0.529, 0.169, 0.254, 0.792]
pose_6 = [-0.865, 0.001, 0.411, 0.286, 0.648, -0.628, -0.321]
pose_list = [home_pose, pose_1, pose_2, pose_3, pose_4, pose_5, pose_6, home_pose]
trajectory = start_state
motion_time = 0
for i, pose in enumerate(pose_list):
goal_pose = Pose.from_list(pose, q_xyzw=False)
start_state = trajectory[-1].unsqueeze(0).clone()
start_state.velocity[:] = 0.0
start_state.acceleration[:] = 0.0
result = motion_gen.plan_single(
start_state.clone(),
goal_pose,
plan_config=MotionGenPlanConfig(parallel_finetune=True, max_attempts=1),
)
if result.success.item():
plan = result.get_interpolated_plan()
trajectory = trajectory.stack(plan.clone())
motion_time += result.motion_time
else:
print(i, "fail", result.status)
print("Motion Time (s):", motion_time)
# CuRobo
from curobo.util.usd_helper import UsdHelper
UsdHelper.write_trajectory_animation(
robot_file,
motion_gen.world_model,
start_state,
trajectory,
save_path="ur5e_sequence.usd",
base_frame="/grid_world_1",
flatten_usd=True,
visualize_robot_spheres=False,
dt=1.0 / 30.0,
)
if __name__ == "__main__":
pose_sequence_ur5e()

79
examples/usd_example.py
View File

@@ -19,6 +19,7 @@ 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.util.logger import setup_curobo_logger
from curobo.util.usd_helper import UsdHelper
from curobo.util_file import (
get_assets_path,
@@ -164,7 +165,81 @@ def read_robot_from_usd(robot_file: str = "franka.yml"):
robot_cfg = RobotConfig.from_dict(robot_cfg, TensorDeviceType())
def save_log_motion_gen(robot_file: str = "franka.yml"):
# load motion generation with debug mode:
robot_cfg = load_yaml(join_path(get_robot_configs_path(), robot_file))["robot_cfg"]
# robot_cfg["kinematics"]["collision_link_names"] = None
world_cfg = WorldConfig.from_dict(
load_yaml(join_path(get_world_configs_path(), "collision_table.yml"))
).get_obb_world()
c_cache = {"obb": 10}
robot_cfg_instance = RobotConfig.from_dict(robot_cfg, tensor_args=TensorDeviceType())
enable_debug = True
motion_gen_config = MotionGenConfig.load_from_robot_config(
robot_cfg_instance,
world_cfg,
collision_cache=c_cache,
store_ik_debug=enable_debug,
store_trajopt_debug=enable_debug,
# num_ik_seeds=2,
# num_trajopt_seeds=1,
# ik_opt_iters=20,
# ik_particle_opt=False,
)
mg = MotionGen(motion_gen_config)
# mg.warmup(enable_graph=True, warmup_js_trajopt=False)
motion_gen = mg
# generate a plan:
retract_cfg = motion_gen.get_retract_config()
state = motion_gen.rollout_fn.compute_kinematics(
JointState.from_position(retract_cfg.view(1, -1))
)
link_chain = motion_gen.kinematics.kinematics_config.link_chain_map[
motion_gen.kinematics.kinematics_config.store_link_map.to(dtype=torch.long)
]
# exit()
link_poses = state.link_pose
# print(link_poses)
# del link_poses["tool0"]
# del link_poses["tool1"]
# del link_poses["tool2"]
# del link_poses["tool3"]
# print(link_poses)
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)
# get link poses if they exist:
result = motion_gen.plan_single(
start_state,
retract_pose,
link_poses=link_poses,
plan_config=MotionGenPlanConfig(max_attempts=1, partial_ik_opt=False),
)
UsdHelper.write_motion_gen_log(
result,
robot_cfg,
world_cfg,
start_state,
retract_pose,
join_path("log/usd/", "debug"),
write_robot_usd_path=join_path("log/usd/", "debug/assets/"),
write_ik=True,
write_trajopt=True,
visualize_robot_spheres=False,
grid_space=2,
link_poses=link_poses,
)
if __name__ == "__main__":
# save_curobo_world_to_usd()
save_curobo_robot_world_to_usd("franka.yml")
setup_curobo_logger("error")
save_log_motion_gen("franka.yml")
# save_curobo_robot_world_to_usd("franka.yml")