NVlabs / curobo

CUDA Accelerated Robot Library
https://curobo.org
Other
798 stars 125 forks source link

how to use cumotion with multi plan group #349

Open frankchan12138 opened 4 months ago

frankchan12138 commented 4 months ago

Hello,I have build up a robot urdf with two arms. I launch moveit setup assistant to define two plan group for each arm. i.e. left_arm and right_arm(each arm have 7 joints). But when I try to use cumotion to plan for left_arm, the cumotion planner node announced that right_shoulder_pitch(one joint of right_arm), is not in the list.

[INFO] [1722319945.724710033] [cumotion_action_server]: Loaded robot file name: /mnt/ssd/nvq_ws/src/pd_arm_description/urdf/pdnew_dualArm_realJointLimits_gripper.xrdf [INFO] [1722319945.725332518] [cumotion_action_server]: warming up cuMotion, wait until ready [INFO] [1722319951.406576925] [cumotion_action_server]: cuMotion is ready for planning queries! [INFO] [1722319975.805609540] [cumotion_action_server]: Executing goal... [INFO] [1722319975.806164186] [cumotion_action_server]: Planning with time_dilation_factor: 0.1 [INFO] [1722319976.261103893] [cumotion_action_server]: Calculating goal pose from Joint target [ERROR] [1722319976.263129040] [cumotion_action_server]: Error raised in execute callback: 'right_shoulder_pitch' is not in list Traceback (most recent call last): File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/action/server.py", line 332, in _execute_goal execute_result = await await_or_execute(execute_callback, goal_handle) File "/opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/executors.py", line 107, in await_or_execute return callback(*args) File "/mnt/ssd/cumotion_ws/install/isaac_ros_cumotion/lib/python3.10/site-packages/isaac_ros_cumotion/cumotion_planner.py", line 561, in execute_callback goal_state = self.motion_gen.get_active_js( File "/mnt/ssd/cumotion_ws/install/curobo_core/lib/python3.10/site-packages/curobo/wrap/reacher/motion_gen.py", line 2082, in get_active_js opt_js = in_js.get_ordered_joint_state(opt_jnames) File "/mnt/ssd/cumotion_ws/install/curobo_core/lib/python3.10/site-packages/curobo/types/state.py", line 393, in get_ordered_joint_state new_js.inplace_reindex(ordered_joint_names) File "/usr/lib/python3.10/contextlib.py", line 79, in inner return func(*args, **kwds) File "/mnt/ssd/cumotion_ws/install/curobo_core/lib/python3.10/site-packages/curobo/types/state.py", line 401, in inplace_reindex new_index_l = [self.joint_names.index(j) for j in joint_names] File "/mnt/ssd/cumotion_ws/install/curobo_core/lib/python3.10/site-packages/curobo/types/state.py", line 401, in <listcomp> new_index_l = [self.joint_names.index(j) for j in joint_names] ValueError: 'right_shoulder_pitch' is not in list

After I read the curobo python file, and print the param, I found that in the follow code new_index_l = [self.joint_names.index(j) for j in joint_names] joint_names have all the 14 joints, while self.joint_names have the left_arm 7 joints. That's why the errror raised.

Now, I want to find out a solution to this problem, any information will be highly appreciated! Thanks a lot!

balakumar-s commented 3 months ago

This feature is currently not supported.

frankchan12138 commented 3 months ago

@balakumar-s Thanks for reply. Right now I have another question, I have tried to manager the two arms into one group. And the curobo would work for that. But when I set the joint target for the whole group, I found that the plan result will not reach the certain target I set. I wonder if there is a solution to figure the planner, which will finally make the planner plan to the exact joint target. Thanks!!