Closed maximiliangilles closed 4 months ago
balakumar-s
commented
7 months ago It looks like warmup failed. Can you warmup with an empty world and then add your world?
You can do that by:
collision_cache = {"obb": 10} to MotionGenConfig.load_from_robot_configmotion_gen.clear_world_cache()motion_gen.update_world(world_config)
maximiliangilles
commented
7 months ago Hi @balakumar-s ,
thanks for the fast reply. I tried your proposed answer, however I still get the same error message. I attached my code for you to verify. Do you see any mistakes?
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.295, 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.4375, 0.19, 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.4375, -0.19, 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
collision_cache = {"obb": 9},
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
use_cuda_graph=False,
collision_activation_distance=collision_activation_distance)
self.motion_gen = MotionGen(motion_gen_cfg)
self.motion_gen.clear_world_cache()
self.motion_gen.warmup()
self.motion_gen.update_world(world_config)
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())Is it possible for you to share the full python file including the imports?
maximiliangilles
commented
7 months ago Hi @balakumar-s ,
sure. See attached my code:
# Third Party
import torch
import time
import copy
from flask import Flask, request, jsonify
# CuRobo
from curobo.geom.types import Cuboid, WorldConfig
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.robot import JointState
from curobo.util.usd_helper import UsdHelper, load_yaml
from curobo.wrap.reacher.motion_gen import MotionGen, MotionGenConfig, PoseCostMetric, MotionGenPlanConfig
from curobo.geom.sdf.world import CollisionCheckerType
from curobo.util_file import join_path, get_world_configs_path
app = Flask(__name__)
# In-memory storage for simplicity; in a real application, consider using a database.
data_store = {}
class MotionGenCUROBO:
def __init__(self, constrain_grasp_approach: bool = False):
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
self.robot_config = load_yaml("/home/curobo/src/curobo/content/configs/robot/franka.yml")
self.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.295, 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.4375, 0.19, 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.4375, -0.19, 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
Cuboid(name="slide", pose=[0.43, 0.36, 0.07, 1., 0., 0., 0.], dims=[0.18, 0.13, 0.14])] #x,y,z, qx, qy, qz, qw
world_config = WorldConfig(cuboid=self.cuboids)
# create motion gen with a cuboid cache to be able to load obstacles later:
motion_gen_cfg = MotionGenConfig.load_from_robot_config(
self.robot_config,
world_config,
self.tensor_args,
trajopt_tsteps=34, # 34
collision_cache = {"obb": 9},
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
use_cuda_graph=True,
collision_activation_distance=collision_activation_distance)
self.motion_gen = MotionGen(motion_gen_cfg)
self.motion_gen.clear_world_cache()
self.motion_gen.warmup()
self.motion_gen.update_world(world_config)
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=50,
enable_graph_attempt=None,
enable_finetune_trajopt=True,
partial_ik_opt=False,
parallel_finetune=False,
pose_cost_metric=pose_metric,
)
def plan(self, joints_start, pose_goal, increased_height: bool = False):
t0 = time.time()
# start state
q_start = JointState.from_position(
self.tensor_args.to_device([[
joints_start['panda_joint1'],
joints_start['panda_joint2'],
joints_start['panda_joint3'],
joints_start['panda_joint4'],
joints_start['panda_joint5'],
joints_start['panda_joint6'],
joints_start['panda_joint7']]]),
joint_names=[
"panda_joint1",
"panda_joint2",
"panda_joint3",
"panda_joint4",
"panda_joint5",
"panda_joint6",
"panda_joint7"])
# goal pose
goal_pose = Pose(
position=self.tensor_args.to_device([[
pose_goal['x'],
pose_goal['y'],
pose_goal['z']]]),
quaternion=self.tensor_args.to_device([[
pose_goal['qw'],
pose_goal['qx'],
pose_goal['qy'],
pose_goal['qz']]]))
# plan
if self.constrain_grasp_approach:
result = self.motion_gen.plan_single(q_start, goal_pose, self.plan_config.clone())
else:
result = self.motion_gen.plan_single(q_start, goal_pose)
t1 = time.time()
if result.success.item():
# this contains a linearly interpolated trajectory with fixed dt
interpolated_solution = result.get_interpolated_plan()
print("Success! Planning time: ", t1 - t0)
return interpolated_solution
else:
print("Failed!")
return False
curobo_obj = MotionGenCUROBO(constrain_grasp_approach=True)
@app.route('/send-data', methods=['POST'])
def send_data():
global data_store
content = request.json
data_store = content
return jsonify({'TrajJoints': f'Got state and goal.'}), 200
@app.route('/get-data/<key>', methods=['GET'])
def get_data(key):
global data_store
if 'start_state' in data_store.keys() and 'goal_pose' in data_store.keys():
# Retrieve the value for the provided key
q_start_dict = data_store['start_state']
goal_pose_dict = data_store['goal_pose']
increased_height = data_store['increased_height']
interpolated_solution = curobo_obj.plan(joints_start=q_start_dict, pose_goal=goal_pose_dict, increased_height=increased_height)
if interpolated_solution:
pos_np = interpolated_solution.position.cpu().numpy()
pos_dict = {}
for step, pos_ in enumerate(pos_np):
pos_dict[str(step)] = [float(value) for value in pos_]
# reset
data_store = {}
return jsonify({key: pos_dict}), 200
else:
return jsonify({key: False}), 200
else:
return jsonify({'message': 'Key not found'}), 404
if __name__ == "__main__":
app.run(debug=True)
And the error message:
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_traj_optim.py", line 171, in get_data
interpolated_solution = curobo_obj.plan(joints_start=q_start_dict, pose_goal=goal_pose_dict, increased_height=increased_height)
File "/home/curobo/examples/motion_gen_api_example_traj_optim.py", line 135, 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 - - [29/Apr/2024 01:45:26] "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_traj_optim.py", line 171, in get_data
interpolated_solution = curobo_obj.plan(joints_start=q_start_dict, pose_goal=goal_pose_dict, increased_height=increased_height)
File "/home/curobo/examples/motion_gen_api_example_traj_optim.py", line 135, 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 reraiseAgain, thanks a lot for your support.
Best,
Maximilian
maximiliangilles
commented
6 months ago Hi @balakumar-s,
I wanted to followup on this issue. Any idea or update, if this will be fixed in future releases?
Thanks a lot! I really appreciate your work and effort.
Best,
Max
balakumar-s
commented
6 months ago Hi, does your code require any additional input to generate your error? I can debug by running your code but currently it looks like it needs additional information.
balakumar-s
commented
4 months ago We have changed the graph so that warmup is not required before adding a waypoint. This should fix your issue. Please re-open if it doesn't.
maximiliangilles
commented
4 months ago Thanks for letting me know, I will report when I run the script again.
Hi,
thanks for your great work! I am trying to run _constrained_motionplanning.
My example code looks like this:
I get the follwing error:
I already tried to set
use_cuda_graph=Falseor comment outmotion.gen_warmup()(see https://github.com/NVlabs/curobo/issues/219). Both possbile fix did not work and throw the same error message.System:
Thank you very much!