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)):