curobo
curobo copied to clipboard
Runtime Error: Constrained Motion Planning
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:
- cuRobo installation mode: docker python
- python version: 3.10.12
Thank you very much!