Obstacle avoidance not working in moveit with python code

[Shantanu Thakar]

Hi All,

I am using the STOMP algorithm for motion planning of a UR5 with a gripper in moveit. Obstacle avoidance works perfectly when I use RVIZ GUI to plan(by running demo.launch in ur5_moveit_config). But when I do exactly the same thing in my code, by attaching objects to the scene(base_link), moveit does not detect obstacles and plans through them. I am using the following function to plan to a Target joint state.

def move_to_target(target):

moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("manipulator")

rospy.sleep(2)
group.set_planning_time(25)
p = PoseStamped()
rospy.loginfo('p defined as PoseStamped')

p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = -0.6
p.pose.position.z = 0.01
scene.attach_box("base_link", "table_base", p, (1,0.5,0.02))
p.pose.position.x = 0.3
p.pose.position.y = -0.6
p.pose.position.z = 0.25
scene.attach_box("base_link", "left_wall", p, (0.02,0.5,0.5))
p.pose.position.x = -0.3
p.pose.position.y = -0.6
p.pose.position.z = 0.25
scene.attach_box("base_link", "right_wall", p, (0.02,0.5,0.5))
rospy.loginfo('objects attached to base_link')

group_variable_values = group.get_current_joint_values()
# Workaround for setting current state as start state as set_start_state is not working as intended
current_state = robot.get_current_state()
joint_name = current_state.joint_state.name
joint_values = current_state.joint_state.position
joint_command = {name:value for name, value in zip(joint_name, joint_values) if name.startswith('manipulator')}
joint_state = JointState()
joint_state.header = Header()
# joint_state.header.stamp = rospy.Time.now()
joint_state.name = joint_command.keys()
joint_state.position = joint_command.values()
moveit_robot_state = RobotState()
moveit_robot_state.joint_state = joint_state
group.set_start_state(moveit_robot_state)

target_joint_values = target
group.set_joint_value_target(target_joint_values)
rospy.loginfo( 'About to plan')

plan = group.plan()
rospy.loginfo('Planning done, now sleeping for 3 secs')

rospy.sleep(3)
rospy.loginfo('Plan should execute now')
group.execute(plan)
rospy.loginfo('Plan executed')

This works perfectly with STOMP in the absence of obstacles, but not with obstacles. Since, obstacle avoidance is working fine in RVIZ, I think I am missing something here. Can anyone please tell me what it is ?

Thanks !

[Clément BAZERQUE]

I am currently working with MoveIt! in python too.

It seems that there is almost no documentation about it...

For me obstacle avoidance work (I am using a RRTconnect). 

I don't know about scene.attach_box() maybe you should try scene.add_box()

I hope it will help...