How to consider tools' spheres while attaching objects to robot?

[linhy97]

Hello, when i use the function "attach_object_to_robot()"
I note that the statements about the parameter "link_name" is: Name of the link (frame) to attach the objects to. The assumption is that this link does not have any geometry and all spheres of this link represent attached objects.
However, in my situation, there exists an tool attached to the UR10's ee_link (for example, a sponge suction cup used to draw something), is there any method to consider the tool's spheres in collision checker at the same time? In my practice, it seems when i attach the object to the tool link, then only spheres of the attached object will be consider and the tool's original spheres will disapear.

[balakumar-s]

Yes, the assumption is that the link to attach spheres should not contain any geometry. You can create an additional frame for your robot like how it's done in

curobo/src/curobo/content/configs/robot/franka.yml

Line 104 in a027cbc

extra_links: {"attached_object":{"parent_link_name": "panda_hand" ,

In addition to creating the link, you also need to initialize spheres for the link like here:

curobo/src/curobo/content/configs/robot/franka.yml

Line 54 in a027cbc

extra_collision_spheres: {"attached_object": 4}

[linhy97]

Thanks~
I try to add extra_links to the end_effector_tool like this:

extra_links: {"attached_object":{"parent_link_name": "end_tool" ,
    "link_name": "attached_object", "fixed_transform": [0,0,0,1,0,0,0], "joint_type":"FIXED",
    "joint_name": "attach_joint" }}
extra_collision_spheres: {"attached_object": 8}

and it works.

Here it seems “attached_object” means an virtual link when nothing attached to the robot, and when we use attach_object to add something to this link, it will transfer to the object's sphere geometry.

I also have another question about {"attached_object": 8}. Here, the number 8 means using 8 spheres to replace an attached object or it can attach at most 8 objects?

[linhy97]

Another problem i meet is that when i use motion_gen.attach_objects_to_robot() to attach the object to robot's end_effector_tool and use write_trajectory_animation_with_robot_usd() to plot and visual the trajectory in isaac sim, the object doesn't move with the robot.

The related code is listed below:

# attach operation
grasp_js = JointState(
    position= tensor_args.to_device(trajectory[-1].position),
    velocity= tensor_args.to_device(trajectory[-1].velocity) * 0.0,
    acceleration= tensor_args.to_device(trajectory[-1].velocity) * 0.0,
    jerk = tensor_args.to_device(trajectory[-1].velocity) * 0.0,
    joint_names= er20_joint_names
)
motion_gen.attach_objects_to_robot(
    grasp_js,
    ['obs_package_grasp'],
    link_name = 'attached_object',
    sphere_fit_type=SphereFitType.VOXEL_VOLUME_SAMPLE_SURFACE,
    world_objects_pose_offset=Pose.from_list([0, 0, 0.01, 1, 0, 0, 0], tensor_args),
)

Here, the obs_package_grasp is the name of Cuboid defined by:
obstacle_package_grasp = Cuboid(name='obs_package_grasp', pose=[1, -0.2, 0.34, 1,0,0,0], dims=[0.3,0.35,0.2], color=[0.7,0.4,0.2,0.9])

I also find the “attached_object” spheres in robot cfg seems no change before and after attaching because i print motion_gen.robot_cfg, like this:

# before
'attached_object': [{'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}]}

# after
'attached_object': [{'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}, {'center': [0.0, 0.0, 0.0], 'radius': -10.0}]}

I think after attaching, the motion_gen should change, but i don't find how to write a new config (robot with attached object), could you tell me the write function? If i write a new yaml file, and pass it to the UsdHelper.write_trajectory_animation_with_robot_usd(), maybe it works?
I have tried like these but failed:

from curobo.util_file import write_yaml
write_yaml(motion_gen.robot_cfg, file_path='./attached_robot.yml')
motion_gen.robot_cfg.kinematics.write_config('./attach_robot.yml')

[qingquluofeng]

Hello, have you solved it? I encountered the same issue when using the attach_objects_to_robot() function.
After using attach_objects_to_robot, it seems that the planned robot trajectory does not take the attached object into consideration.

[linhy97]

A simple method to check if it works is:
print motion_gen.robot_cfg.kinematics.kinematics_config.link_spheres info before and after operating attach_objects_to_robot().

For example, in my situation, the spheres of attached_object link is initial like [ 0.0000e+00, 0.0000e+00, 0.0000e+00, -1.0000e+02]. It means just a virtual link that don't have any collision property.
And after attaching an object to this virtual link, the corresponding spheres are given some specific values like [-1.5107e-01, 4.5794e-02, 1.3618e-01, 5.0000e-03]. It means the real collision representation of attached object.

Furthermore, if you find the attach operation doesn't work, you could check:
1）yml config file settings. You should explicitly assign an extra_link. You can refer to the tutorials Configuring a New Robot
2）parameters of attach_objects_to_robot(). For example, the link_name is right and is corresponding to the name in the yml file.

[qingquluofeng]

Thank you ^-^.

[AmirpooyaSh]

Hello @linhy97 and @qingquluofeng

The Attach/Detach functions will create a sphere representation of the selecting object on the virtual link (or tool) on the robot and updates the whole robot's sphere representation (As I tested the CuRobo itself using MoveIt and in ROS-Noetic).

The problem you're encountering is that the environment represented for CuRobo (contains a set of Cuboids, Meshes, ....) is not equal to the Isaac Sim's world. It basically means that everytime you're trying to plan a movement for your robot, you're updating the CuRobo's world representation using USD_Helper library which acts like an API to get the information from Isaac Sim and update CuRobo's world with that.

Therefore, there is no real-time communication between CuRobo's world representation and Isaac Sim (it might be in CuRobo's MPC function, but I haven't gone deep into it yet).

Now, once you attach an object to the robot, what happens is that the object's sphere representation will be generated and added to the created virtual link (which updates the whole robot's sphere representation in CuRobo). This will result in a correct movement of the robot (when you're using path planning functions such as plan_single(...)).

However, you won't see an actual attachment of the object to the robot in your simulation environment (Isaac Sim for your case or Gazebo in other cases). Therefore, you can proceed to the solution with 2 different approaches:

1- The simple solution is to remove the object from the environment and create it in the virtual link's location once the path planning is done (It seems dumb since you can't see the actual movement of the robot + object). However, it works fine since the sphere representation of the robot has the object's sphere representation added to itself (Because the whole point of CuRobo is to move the robot's sphere in a way to avoid collision with the world environment).

2- If you want to see the actual movement of the robot and object together, you have to not only attach the object to the robot in CuRobo (the sphere representation of it), but also attach the object to the robot in the simulation environment (creating a new virtual link in Isaac Sim). To do so, you have to use the Isaac Sim's Surface Gripper extension. It will create an additional link for the robot (it can be in the same location as the virtual link or tool created for CuRobo) and it will attach and lock any object that hits the coordination on Close/Open impulses. I will add a couple of links below which are good starting points to this matter.

Omniverse Isaac Sim Gripper Extension

Surface Gripper Medium Tutorial

Just a quick tip:
The simple solution is to create a Surface Gripper OmniGraph in your Python code (if you're using python standalone) and give your closing/opening impulses a meaningful name. Therefore, you can attach the object to the robot in Isaac Sim once you attached it in CuRobo (by sending impulses to the corresponding OmniGraph node). It can also be done with omni.kit.execution functions.

We have already used this extension in our Python standalone code and it's working. However, since we haven't published the work yet, I can't provide any piece of our code as of now. You can get into it and let me know if you had any problems, so I will help you on that matter if you want to ^-^.

Best of Luck

[qingquluofeng]

Thank you ^-^.

[linhy97]

@AmirpooyaSh Thanks a lot for your detailed answer~
I think we can say: if we want to see the dynamic trajectory and other real-time operation of Curobo's results, we should use the Isaac Sim to construct an environment and dynamic update parameters.
Emm.., and sometimes it seems a little heavy, if we want to execute some complex tasks (/ω＼). Maybe using Pybullet or other simulation library to display the result is also a good alternative.

[AmirpooyaSh]

That's right.
In our case, we're using 2 robots (2 seperate CuRobo instances) in a single Isaac Sim environment. Each robot does have 2 to 3 tools on its end effector.

That having been said, activating 2 MotionGenConfigs (each with 1 eef_link which we frequently change and rewarmup the CuRobo) with the world environment of 100 Meshes and 30 Cuboids (cache value) is occupying almost 10GB of our GPU's VRam.

The main issue with CuRobo (or Pytorch in general) is that by updating the tool and rewarming up the MotionGen, the previous config still occupies the VRam until the new one gets warmed up, so you need like a 2GB of VRam offset in addition to your occupied VRam.

Long story short, you have to think about "A" serie GPUs for bringing up CuRobo + Isaac Sim within complex environments. On the other hand, you can work in your alternative simulation of your choice, but you have to basically rewrite the whole function updating CuRobo's world representation with the corresponding simulation (helper.py which has several functions in it to communicate with Isaac Sim).

[linhy97]

Another problem i meet is how to find an appropriate settings of attaching objects, i find a larger number(e.g. 256) of parameter attached_object in robot yml file will bring more calculate time obviously, but a smaller number maybe insufficient to represent the attached object?
Briefly, i think the number of spheres means trade-off between modeling accuracy and computational efficiency.
And i am confused about two problems:

1. In the pick and place tasks, how to consider the attached object's collision geometry better? Just setting the parameter extra_collision_spheres in yml file like this: {"attached_object": 8}? and adjusting the parameter surface_sphere_radius of function attach_objects_to_robot()? Maybe a smaller number of attached_object should match a larger surface_sphere_radius?
2. Furthermore, is the mechanism of spheres approximation efficient for attached cubic objects? Sometimes approximating an attached cube into many small spheres(e.g. 256 or more spheres) will cost more computational time than just keeping it as a cube?
   Hope for anyone's help, thanks~

[AmirpooyaSh]

@linhy97

You're right. The more computational power you have the faster you can reach into a solution (specially upon attaching/detaching objects).
Another factor that can be considered as such tradeoffs is the number of seeds being created for the motion generation object. It's set to 32 as a default value for the inverse kinematic. However, you can reduce this number to reduce the computational power and therefore have a slower runtime to find the solution.

The matter of sufficiency for your object's sphere representation upon attaching is also dependent to the object's shape.
Briefly, you can take a look at the figure provided on CuRobo's world representation page (It visualizes different methods of SphereFitType on 2 mesh shapes). => Link

In our case, we're working with wooden materials that are almost in a cubic shape, but with long lengths compared to their width/height (An example is a 0.1m x 0.05m x 3.0m wooden stud).

To redefine it as spheres, we've seen that the only SphereFitTypes working well were "VOXEL_VOLUME_SAMPLE_SURFACE" and "SAMPLE_SURFACE" with small radiuses (like 0.001m). It still has some insufficiencies on the edges of our picking materials (can't fit well), but it's working just fine!

Right now, we're thinking of writing a custom Voxelization method for our material to satisfy our needs. You can do this as well to reach a better representation of your grasping object (and maybe optimize the number of spheres).

As a rule of thumb, "VOXEL_VOLUME" and "VOXEL_VOLUME_INSIDE" with a lower amount of spheres and larger radius sizes are good for objects having symmetric dimensions in different directions (Like a cup, a box, ....).
However, objects that have 1 dimension << other dimensions need small sphere radiuses (like 0.001m), and "SAMPLE_SURFACE" and "VOXEL_VOLUME_SAMPLE_SURFACE" voxelization methods to be represented better (and obviously with more number of spheres like 100-150).

[linhy97]

Hi~ @AmirpooyaSh
I think i find an easy but not very accurate way to visualize the attached object by adding the approximated spheres to the ee_link or end_tool of robot arm.

Some reference code like this:

# [1]  record the spheres
# record the initial spheres, now the attached_object's spheres are empty.
spheres_before = motion_gen.robot_cfg.kinematics.kinematics_config.link_spheres.cpu().numpy().copy()

# attach operation
motion_gen.attach_objects_to_robot(blabla...)

# record the spheres after attaching, now the attached_object's spheres are concrete
spheres_after = motion_gen.robot_cfg.kinematics.kinematics_config.link_spheres.cpu().numpy().copy()

# [2]  update a new yml_file with newly attached spheres
file_name = 'xxx.yml'
dict_robot_cfg = load_yaml(file_name)
list_spheres_new = dict_robot_cfg['robot_cfg']['kinematics']['collision_spheres']['end_tool']
list_link_sphere_idx_map = motion_gen.robot_cfg.kinematics.kinematics_config.link_sphere_idx_map.cpu().tolist()
start_idx = 0
for i in range(len(list_link_sphere_idx_map)):
    if list_link_sphere_idx_map[i] == max(list_link_sphere_idx_map):
        start_idx = i
        break
print("spheres start idx of attahced object",start_idx)
for sphere_ in spheres_after[start_idx:]:
    dict_sphere_cur = {
        'center': sphere_[0:3].tolist(),
        'radius': sphere_[3]
    }
    list_spheres_new.append(dict_sphere_cur)
dict_robot_cfg['robot_cfg']['kinematics']['collision_spheres']['end_tool'] = list_spheres_new

# [3] visualize the animation, use the new `dict_robot_cfg`
UsdHelper.write_trajectory_animation_with_robot_usd(dict_robot_cfg, blabla...)

By attaching the voxel spheres to end_tool's collision spheres, we tricky solve the problem. It will helps us to check some errors. For example, we can check whether the number of spheres are sufficient for various objects.

[AmirpooyaSh]

@linhy97
That's a cool way TBH.
I'm doing this by using a piece of code they provided within "curobo/examples/isaac_sim/motion_gen_reacher.py"

        if args.visualize_spheres and step_index % 2 == 0:
            sph_list = motion_gen.kinematics.get_robot_as_spheres(cu_js.position)

            if spheres is None:
                spheres = []
                # create spheres:

                for si, s in enumerate(sph_list[0]):
                    sp = sphere.VisualSphere(
                        prim_path="/curobo/robot_sphere_" + str(si),
                        position=np.ravel(s.position),
                        radius=float(s.radius),
                        color=np.array([0, 0.8, 0.2]),
                    )
                    spheres.append(sp)
            else:
                for si, s in enumerate(sph_list[0]):
                    if not np.isnan(s.position[0]):
                        spheres[si].set_world_pose(position=np.ravel(s.position))
                        spheres[si].set_radius(float(s.radius))

Once you call the attach_object_to_robot(...) function, it will automatically update the sphere representation of the links (basically adding the sphere representation of the attached object). Therefore, having the above piece of code in the execution function of your path planning algorithm will make it work (the moment you're passing waypoints to the robot's articulation controller).

Just to let you know, if you're robot is not in [0,0,0] and you want to add_robot_to_scene(...) with a different origin (for our case we had a second robot in [4.6, 0, 0.025] location, the robot will be placed, but the spheres will be shown on [0,0,0] if you use the code above. To make it work, you have to update sphere poses in the loops position=np.ravel(s.position) | spheres[si].set_world_pose(position=np.ravel(s.position)) with the robot's location (in the case I mentioned you have to add 4.6 to their x values and 0.025 to their z values).