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')