Revision history [back]

After long hours of puzzling over the problem I found a working solution. Perhaps it is not the most elegant solution but it works...

I get the attached-objects msg from the planning scene and appends it at the robot state. After that I had to modifiy the mesh_poses and frame_id to place the object in the right pose (not shown in the code fragment).

# Attach part for creating attached object msg
tubehandling.attach_part(tubename)
attached_objects = tubehandling.scene.get_attached_objects()
tubehandling.detach_part(tubename)
# Remove part to prevent collision in planning
tubehandling.remove_part(tubename)

# Update the robot state with attached objects
robot_start_state = tubehandling.robot.get_current_state()
robot_start_state.joint_state.position = robot_state

# Add attached object to robot state
robot_start_state.attached_collision_objects.append(attached_objects[tubename])

I just attached one object.

I hope the code fragment helps... Thanks!