ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

How to attach objects in robot state

asked 2021-12-01 01:45:45 -0500

nicob gravatar image

updated 2021-12-02 01:38:43 -0500

I am planning a robot motion in two steps. So I can modify the boundary conditions of the target pose of the first step (orienation of gripper), if the planning of step two fails (bring part to target pose). Background is to handle parts of high varity in geometry without teaching.

In a first development step I planned AND executed the steps subsequently. All motions worked fine and there were no collisions. Now, I observe some collisions in the second step in RViz but the motion is executed without recognition of the collision. Furthermore all of the end poses are correct. Planning algorithm is the same.

Some background info of my code: I plan the first motion in classic way with

plan1 = tubehandling.move_group.plan()

If plan1[0] == True the next planning step follows. For this I get the joint angles of the last plannend point in plan1 and update with this angels the robot_start_state for the second planning:

target_joints = plan1[1].joint_trajectory.points[-1].positions
target_joints = list(target_joints)
robot_state = tubehandling.robot.get_current_state().joint_state.position
robot_state = list(robot_state)
# Update joint angles with planned robot state at gripping point
robot_state[0:6] = target_joints[0:6]
print("Robot end state (angles):")

robot_state = tuple(robot_state)
robot_start_state = tubehandling.robot.get_current_state()
robot_start_state.joint_state.position = robot_state

# Set new start state for 2nd planning

After this the planning follows WITH attached part and if plan2[0] == True the two plans are carried out.

plan2 = tubehandling.move_group.plan()

Do I overlook something? Do I have to update somethin else? As usual, thanks in advance!

edit retag flag offensive close merge delete


It was possible for me to pinpoint the fault more. With the MotionPlanning - Trajectory Slider in RViz I recognized that the part is not correctly attached to the robot. I do not understand why it makes a difference if I attach the part to the robot when I am planning step by step. The function I am using is the same:

# Attach a part to the robot
def attach_part(self, part_name, timeout=4):
    print("Attaching following object:")


    # Wait for the planning scene to update.
    return self.wait_for_state_update( part_name,
        object_is_attached=True, object_is_known=False, timeout=timeout

Any ideas? Thanks!

nicob gravatar image nicob  ( 2021-12-01 04:56:11 -0500 )edit

print(tubehandling.scene.get_attached_objects()) shows the attached object correctly in the second planning.

nicob gravatar image nicob  ( 2021-12-01 07:50:50 -0500 )edit

One more comment...

I think it is definitv a problem of an incomplete definition of the robot_state msg. The command robot_start_state = tubehandling.robot.get_current_state() seems not to return the attached objects in contrast to tubehandling.move_group.set_start_state_to_current_state(), which is setting the attached objects correct accordingly to the planning scene.

Is there a comfortable way to update the attached objects to the robot_state msg from the planning interface by hand? Thanks!

nicob gravatar image nicob  ( 2021-12-01 08:52:56 -0500 )edit

Hello @nicob,

Instead of adding multiple comments, I would suggest re-editing your question.

Ranjit Kathiriya gravatar image Ranjit Kathiriya  ( 2021-12-02 03:02:33 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2021-12-02 06:57:30 -0500

nicob gravatar image

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
attached_objects = tubehandling.scene.get_attached_objects()
# Remove part to prevent collision in planning

# 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

I just attached one object.

I hope the code fragment helps... Thanks!

edit flag offensive delete link more

Question Tools



Asked: 2021-12-01 01:45:45 -0500

Seen: 212 times

Last updated: Dec 02 '21