Runtime Error: Constrained Motion Planning

[maximiliangilles]

Hi,

thanks for your great work! I am trying to run constrained_motion_planning.

My example code looks like this:

self.constrain_grasp_approach = constrain_grasp_approach
self.tensor_args = TensorDeviceType(device=torch.device("cuda:0"))
interpolation_dt = 0.02
collision_activation_distance = 0.02  # meters
# create
robot_config = load_yaml("/home/curobo/src/curobo/content/configs/robot/franka.yml")
cuboids = [
    Cuboid(name="bin1", pose=[0.58, 0.0, 0.035, 1., 0., 0., 0.], dims=[0.01, 0.4, 0.07]),  #x,y,z, qx, qy, qz, qw
    Cuboid(name="bin2", pose=[0.28, 0.0, 0.035, 1., 0., 0., 0.], dims=[0.01, 0.4, 0.07]),  #x,y,z, qx, qy, qz, qw
    Cuboid(name="bin3", pose=[0.45, 0.2, 0.035, 1., 0., 0., 0.], dims=[0.3, 0.01, 0.07]),  #x,y,z, qx, qy, qz, qw
    Cuboid(name="bin4", pose=[0.45, -0.2, 0.035, 1., 0., 0., 0.], dims=[0.3, 0.01, 0.07]),  #x,y,z, qx, qy, qz, qw
    # Cuboid(name="bin", pose=[0.45, 0.0, 0.035, 1., 0., 0., 0.], dims=[0.3, 0.4, 0.07]),  #x,y,z, qx, qy, qz, qw
    Cuboid(name="table", pose=[0.0, 0.0, -0.05, 1., 0., 0., 0.], dims=[5.0, 5.0, 0.1]),  #x,y,z, qx, qy, qz, qw
    Cuboid(name="column1", pose=[0.84, 0.36, 0.5, 1., 0., 0., 0.], dims=[0.08, 0.08, 1.0]),
    Cuboid(name="column2", pose=[0.84, -0.36, 0.5, 1., 0., 0., 0.], dims=[0.08, 0.08, 1.0]),
    Cuboid(name="column3", pose=[-0.26, 0.36, 0.5, 1., 0., 0., 0.], dims=[0.08, 0.08, 1.0]),
    Cuboid(name="column4", pose=[-0.26, -0.36, 0.5, 1., 0., 0., 0.], dims=[0.08, 0.08, 1.0])]  #x,y,z, qx, qy, qz, qw

world_config = WorldConfig(cuboid=cuboids)
# create motion gen with a cuboid cache to be able to load obstacles later:
motion_gen_cfg = MotionGenConfig.load_from_robot_config(
    robot_config,
    world_config,
    self.tensor_args,
    trajopt_tsteps=34, # 34 
    interpolation_steps=5000,
    num_ik_seeds=50,
    num_trajopt_seeds=6,
    grad_trajopt_iters=500,
    trajopt_dt=0.5,
    interpolation_dt=interpolation_dt,
    evaluate_interpolated_trajectory=True,
    js_trajopt_dt=0.5,
    js_trajopt_tsteps=34,  # 34
    collision_activation_distance=collision_activation_distance)
self.motion_gen = MotionGen(motion_gen_cfg)
self.motion_gen.warmup()
if self.constrain_grasp_approach:
    pose_metric = PoseCostMetric.create_grasp_approach_metric(
        offset_position=0.05, tstep_fraction=0.2, linear_axis=2
    )
    self.plan_config = MotionGenPlanConfig(
        enable_graph=False,
        max_attempts=10,
        enable_graph_attempt=None,
        enable_finetune_trajopt=True,
        partial_ik_opt=False,
        parallel_finetune=False,
        pose_cost_metric=pose_metric,
    )
result = self.motion_gen.plan_single(q_start, goal_pose, self.plan_config.clone())

I get the follwing error:

tensor([ 250., 5000.,   20.,   20.], device='cuda:0')
Updating offset waypoint is only possible after initializing motion gen run motion_gen.warmup() before adding offset_waypoint
NoneType: None
Stack (most recent call last):
  File "/usr/lib/python3.10/threading.py", line 973, in _bootstrap
    self._bootstrap_inner()
  File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner
    self.run()
  File "/usr/lib/python3.10/threading.py", line 953, in run
    self._target(*self._args, **self._kwargs)
  File "/usr/lib/python3.10/socketserver.py", line 683, in process_request_thread
    self.finish_request(request, client_address)
  File "/usr/lib/python3.10/socketserver.py", line 360, in finish_request
    self.RequestHandlerClass(request, client_address, self)
  File "/usr/lib/python3.10/socketserver.py", line 747, in __init__
    self.handle()
  File "/usr/local/lib/python3.10/dist-packages/werkzeug/serving.py", line 390, in handle
    super().handle()
  File "/usr/lib/python3.10/http/server.py", line 433, in handle
    self.handle_one_request()
  File "/usr/lib/python3.10/http/server.py", line 421, in handle_one_request
    method()
  File "/usr/local/lib/python3.10/dist-packages/werkzeug/serving.py", line 362, in run_wsgi
    execute(self.server.app)
  File "/usr/local/lib/python3.10/dist-packages/werkzeug/serving.py", line 325, in execute
    for data in application_iter:
  File "/usr/local/lib/python3.10/dist-packages/werkzeug/debug/__init__.py", line 330, in debug_application
    app_iter = self.app(environ, start_response)
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 1498, in __call__
    return self.wsgi_app(environ, start_response)
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 1473, in wsgi_app
    response = self.full_dispatch_request()
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 880, in full_dispatch_request
    rv = self.dispatch_request()
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 865, in dispatch_request
    return self.ensure_sync(self.view_functions[rule.endpoint])(**view_args)  # type: ignore[no-any-return]
  File "/home/curobo/examples/motion_gen_api_example.py", line 180, in get_data
    interpolated_solution = curobo_obj.plan(joints_start=q_start_dict, pose_goal=goal_pose_dict)
  File "/home/curobo/examples/motion_gen_api_example.py", line 138, in plan
    result = self.motion_gen.plan_single(q_start, goal_pose, self.plan_config.clone())
  File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 1322, in plan_single
    result = self._plan_attempts(
  File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 1097, in _plan_attempts
    valid_query = self.update_pose_cost_metric(
  File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 2535, in update_pose_cost_metric
    [
  File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 2536, in <listcomp>
    rollout.update_pose_cost_metric(metric)
  File "/usr/local/lib/python3.10/dist-packages/curobo/rollout/arm_reacher.py", line 467, in update_pose_cost_metric
    [
  File "/usr/local/lib/python3.10/dist-packages/curobo/rollout/arm_reacher.py", line 468, in <listcomp>
    x.update_offset_waypoint(
  File "/usr/local/lib/python3.10/dist-packages/curobo/rollout/cost/pose_cost.py", line 207, in update_offset_waypoint
    log_error(
  File "/usr/local/lib/python3.10/dist-packages/curobo/util/logger.py", line 44, in log_error
    logger.error(txt, exc_info=exc_info, stack_info=stack_info, *args, **kwargs)
127.0.0.1 - - [15/Apr/2024 01:19:35] "GET /get-data/position HTTP/1.1" 500 -
Traceback (most recent call last):
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 1498, in __call__
    return self.wsgi_app(environ, start_response)
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 1476, in wsgi_app
    response = self.handle_exception(e)
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 1473, in wsgi_app
    response = self.full_dispatch_request()
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 882, in full_dispatch_request
    rv = self.handle_user_exception(e)
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 880, in full_dispatch_request
    rv = self.dispatch_request()
  File "/usr/local/lib/python3.10/dist-packages/flask/app.py", line 865, in dispatch_request
    return self.ensure_sync(self.view_functions[rule.endpoint])(**view_args)  # type: ignore[no-any-return]
  File "/home/curobo/examples/motion_gen_api_example.py", line 180, in get_data
    interpolated_solution = curobo_obj.plan(joints_start=q_start_dict, pose_goal=goal_pose_dict)
  File "/home/curobo/examples/motion_gen_api_example.py", line 138, in plan
    result = self.motion_gen.plan_single(q_start, goal_pose, self.plan_config.clone())
  File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 1322, in plan_single
    result = self._plan_attempts(
  File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 1097, in _plan_attempts
    valid_query = self.update_pose_cost_metric(
  File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 2535, in update_pose_cost_metric
    [
  File "/usr/local/lib/python3.10/dist-packages/curobo/wrap/reacher/motion_gen.py", line 2536, in <listcomp>
    rollout.update_pose_cost_metric(metric)
  File "/usr/local/lib/python3.10/dist-packages/curobo/rollout/arm_reacher.py", line 467, in update_pose_cost_metric
    [
  File "/usr/local/lib/python3.10/dist-packages/curobo/rollout/arm_reacher.py", line 468, in <listcomp>
    x.update_offset_waypoint(
  File "/usr/local/lib/python3.10/dist-packages/curobo/rollout/cost/pose_cost.py", line 207, in update_offset_waypoint
    log_error(
  File "/usr/local/lib/python3.10/dist-packages/curobo/util/logger.py", line 45, in log_error
    raise
RuntimeError: No active exception to reraise

I already tried to set use_cuda_graph=False or comment out motion.gen_warmup() (see https://github.com/NVlabs/curobo/issues/219). Both possbile fix did not work and throw the same error message.

System:

1. cuRobo installation mode: docker python
2. python version: 3.10.12

Thank you very much!