mikeferguson / moveit_python

Pure Python Bindings to ROS MoveIt!

moveit_python

This is a set of pure-python bindings to the ROS interface of MoveIt! based on the earlier moveit_utils package that was developed as part of the chess_player package.

Overview

There are three interfaces currently:

• MoveGroupInterface -- used to move the arm using the move_group action.
• PlanningSceneInterface -- used to add/remove collision and attached objects. Can also set the colors of objects.
• PickPlaceInterface -- used to interface to the pick and place actions.

MoveGroupInterface

The MoveGroupInterface is quite easy to use:

import rospy
from moveit_python import *

rospy.init_node("moveit_py")
g = MoveGroupInterface("planning_group_name", "base_link")

Obviously, you might need different values for base_link, and your planning group is probably not called "planning_group_name".

PlanningSceneInterface

The PlanningSceneInterface allows you to easily insert and remove objects from the MoveIt! planning scene. Starting with version 0.3.0, this module will try to use the newer service-based approach to apply planning scene updates, as this is much more robust than publishing messages over topics asynchronously.

import rospy
from moveit_python import *

rospy.init_node("moveit_py")
# create a planning scene interface, provide name of root link
p = PlanningSceneInterface("base_link")

# add a cube of 0.1m size, at [1, 0, 0.5] in the base_link frame
p.addCube("my_cube", 0.1, 1, 0, 0.5)

# do something

# remove the cube
p.removeCollisionObject("my_cube")

If for some reason you would prefer to not use the service, simply set "use_service" to false in the various add/remove calls. Calling waitForSync will republish messages as needed to synchronize the planning scene between your script and MoveIt.

p.addCube("my_cube", 0.1, 1, 0, 0.5, use_service=False)
p.addCube("my_other_cube", 0.1, 2, 0, 0.5, use_service=False)
p.waitForSync()

PickPlaceInterface

import rospy
from moveit_python import *
from moveit_msgs.msg import Grasp, PlaceLocation

rospy.init_node("moveit_py")
# provide arm group and gripper group names
# also takes a third parameter "plan_only" which defaults to False
p = PickPlaceInterface("arm", "gripper")

g = Grasp()
# fill in g
# setup object named object_name using PlanningSceneInterface
p.pickup("object_name", [g, ], support_name = "supporting_surface")

l = PlaceLocation()
# fill in l
p.place("object_name" [l, ], goal_is_eef = True, support_name = "supporting_surface")

Migration from moveit_utils

• GraspingInterface renamed to PickPlaceInterface
  • The pick/place functions now return the entire action result, in moveit_utils they returned only the error_code. To access the error code as it used to be returned, you would use result.error_code.val
• ObjectManager renamed to PlanningSceneInterface
  • remove function is now removeCollisionObject and removeAttachedObject
• ArmInterface renamed to MoveGroupInterface

moveit_python's Issues

PlanningSceneInterface deletes all objects on startup

ObjectManager: sync timed out.

Hi,
I always get the error "ObjectManager: sync timed out." when I use p.addCube("my_cube", 0.1, 1, 0, 0.5)
What is the problem?

Moveit_python in namespaced node does not place the topics in the namespace

I wrote a ROS node that uses both the PlanningSceneInterface and the RobotCommander. The node works as expected when my robot is not namespaced, but since I want to use multiple arms I need to be able to namespace the robots. As soon as I do this, the PlanningSceneInterface does not map its topics to the namespaced topics created by move_group. The RobotCommander does work as expected.

In your code I see no reason the topics should not end up in the namespace as they don't have a leading slash, but somehow the do not. Can you help me out?

Add tests

Add runtime tests to make sure basic features work.

Update the planning scene wrapper with namespace functionality.

Hi, this package provides a better python binding than the official moveit_commander python package. However, I found a need to use the planning scene with specific namespace, which is not really well supported even with the official moveit_commander package. Therefore, I have modify your source code on planning_scene_interface.py slightly with the following. This is to incorporate multiple planning scene for different robot namespace (e.g. robot1/planning_scene or robot2/planning_scene). I find that this merge could be helpful for those who are going to use this functionality.

`
class PlanningSceneInterface(object):
def init(self, frame, ns='', init_from_service=True):
# ns must be a string
if not isinstance(ns,basestring):
rospy.logerr('Namespace must be a string!')

    self._fixed_frame = frame

    # publisher to send objects to MoveIt
    self._pub = rospy.Publisher(ns + '/collision_object',
                                CollisionObject,
                                queue_size=10)
    self._attached_pub = rospy.Publisher(ns + '/attached_collision_object',
                                         AttachedCollisionObject,
                                         queue_size=10)
    self._scene_pub = rospy.Publisher(ns + '/planning_scene',
                                      PlanningScene,
                                      queue_size=10)

    # track the attached and collision objects
    self._mutex = thread.allocate_lock()
    # these are updated based what the planning scene actually contains
    self._attached = list()
    self._collision = list()
    # these are updated based on internal state
    self._objects = dict()
    self._attached_objects = dict()
    self._removed = dict()
    self._attached_removed = dict()
    self._colors = dict()

    # get the initial planning scene
    if init_from_service:
        rospy.loginfo('Waiting for get_planning_scene')
        rospy.wait_for_service(ns + '/get_planning_scene')
        self._service = rospy.ServiceProxy(ns + '/get_planning_scene',
                                           GetPlanningScene)
        try:
            req = PlanningSceneComponents()
            req.components = sum([
                PlanningSceneComponents.WORLD_OBJECT_NAMES,
                PlanningSceneComponents.WORLD_OBJECT_GEOMETRY,
                PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS])
            scene = self._service(req)
            self.sceneCb(scene.scene, initial=True)
        except rospy.ServiceException as e:
            rospy.logerr('Failed to get initial planning scene, results may be wonky: %s', e)

    # subscribe to planning scene
    rospy.Subscriber(ns + '/move_group/monitored_planning_scene',
                     PlanningScene,
                     self.sceneCb)

`

Kinetic: implosion due to pyassimp bug

https://bugs.launchpad.net/ubuntu/+source/assimp/+bug/1589949

Add remove_objects and list_objects scripts

These exist in downstream repositories and should be moved here.

Adding Objects to the scene

Can I add any other object such as cylinder, sphere as such.

PlanningScene AddBox and AddCube use only __fixed_frame

moveit_python/src/moveit_python/planning_scene_interface.py

Line 302 in 2963f35

def addBox(self, name, size_x, size_y, size_z, x, y, z, use_service=True, frame_id=None):

Unlike the other cylinder and sphere primitives, the box and cube primitives are not passing frame_id data back to addSolidPrimitive
They are being set to the fixed frame of the scene rather than having different frames

any tutorial?

frankly, without the tutorial we don't know how to use this....
I am looking forward to seeing the tutorial..

Add variable to planning functions to optionally disable waiting for the response

• Add wait parameter that defaults to True to all planner functions
• Add functions to poll current goal status and query the response if it is finished

secondarily:

• Reduce code duplication where possible
• do some pep8 fixing for good measure

"from pyassimp import pyassimp" in planning_scene_interface.py is correct?

Hello
Acording to pyassimp's README.md,
I think
import pyassimp
is correct rather than "from pyassimp import pyassimp" at line 31 in planning_scene_interface.py.

I've failed launching Moveit! while trying fetch robot demo and get below error

☁  ~  roslaunch fetch_gazebo_demo demo.launch
... 
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://E440:40788/
.
.
.
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/fetch_gazebo_demo/demo.py", line 36, in <module>
    from moveit_python import (MoveGroupInterface,
  File "/opt/ros/indigo/lib/python2.7/dist-packages/moveit_python/__init__.py", line 4, in <module>
    from planning_scene_interface import PlanningSceneInterface
  File "/opt/ros/indigo/lib/python2.7/dist-packages/moveit_python/planning_scene_interface.py", line 31, in <module>
    from pyassimp import pyassimp
ImportError: cannot import name pyassimp
[demo-7] process has died [pid 9461, exit code 1, cmd /opt/ros/indigo/lib/fetch_gazebo_demo/demo.py __name:=demo __log:=/hom

And changing from pyassimp import pyassimp to import pyassimp make demo work correctly.
But this line hasn't been edited long time and I don't know why it worked at past time.
my pyassimp version is 3.3

☁  ~  pip freeze | grep pyassimp
pyassimp==3.3

Sorry for my poor English
Thank you

Is there an option to check collisions - I would like to check if the robot is in collision with any object in the environment

I have been working with the python Interface and I would like to know if there is any possibility to query for collisions in the scene.

While using the cartesian path planning, with a UR5 robot to be specific, on moving close to a cube object for a top down grasp, when I intentionally make the gripper collide with the cube no warning is raised. The gripper just stops when on collision with the cube. Is this normal behavior? If so it would be great to get a functionality to see the details of objects that are in collision in the scene.

How to set "is_diff" in this package

I try to use removeAttachedObject() function to remove the attached object. But the terminal gives me this error.
The specified RobotState is not marked as is_diff. The request to modify the object 'microwave_door1' is not supported. Object is ignored

Where can I set this flag?

Thank you!

PickupActionGoal Attribute Error

I am using move it simple grasps generator to fill the grasps. I am getting the following error :

Traceback (most recent call last):
File "/ros_bx/src/pnp/pnp.py", line 145, in
p.pickup("my_cube", grasp_list)
File "/ros_bx/src/moveit_python/src/moveit_python/pick_place_interface.py", line 154, in pickup
self._pick_action.send_goal(g)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/simple_action_client.py", line 92, in send_goal
self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/action_client.py", line 553, in send_goal
return self.manager.init_goal(goal, transition_cb, feedback_cb)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/actionlib/action_client.py", line 466, in init_goal
self.send_goal_fn(action_goal)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 882, in publish
self.impl.publish(data)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/topics.py", line 1066, in publish
serialize_message(b, self.seq, message)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/msg.py", line 152, in serialize_message
msg.serialize(b)
File "/opt/ros/kinetic/lib/python2.7/dist-packages/moveit_msgs/msg/_PickupActionGoal.py", line 907, in serialize
_v1 = val1.pre_grasp_posture
AttributeError: 'PlaceLocation' object has no attribute 'pre_grasp_posture'