dfki-ni / grasplan

Grasplan: Simple grasp planning for robots

grasplan

Documentation can be found under: https://grasplan-documentation.readthedocs.io/en/latest/

pre-commit Formatting Checks

This repo has a pre-commit check that runs in CI. You can use this locally and set it up to run automatically before you commit something. To install, use pip:

pip3 install --user pre-commit

To run over all the files in the repo manually:

pre-commit run -a

To run pre-commit automatically before committing in the local repo, install the git hooks:

pre-commit install

grasplan's Issues

Use interactive markers to change the pose of objects in a planning scene

To reduce the time needed to create a planning scene, the use of interactive markers would be nice. I already started, with the current state looking like this:

It still has to be integrated into the rqt gui

Rewrite insert object functionality from scratch

• Currently the place obj server is reused by generating 2 place configurations on top of the klt.
• This effectively "drops" the object inside the klt but does not really gently inserts the object inside.
• The ongoing refactoring of the place module is slowly removing all hacks which will make very difficult to keep the current approach alive (and neither we want to...)

So the idea for this project is to develop a new insert object node from scratch, perhaps using the Task constructor (see #2), suggested workflow is as follows:

• robot picks an object
• robot moves arm to place the object on top of the container
• arm performs a linear motion down until a force is sensed
• upon feeling resistance go a bit up and open gripper
• retract arm fully by performing a linear motion up
• done, now the object is inserted gently at the bottom of the container

Publish planning scene at startup

Currently the planning scene, is added with the first pick action:

grasplan/src/grasplan/pick.py

Lines 319 to 333 in f628097

	for planning_scene_box in self.planning_scene_boxes:
	# add a box to the planning scene
	table_pose = PoseStamped()
	table_pose.header.frame_id = planning_scene_box['frame_id']
	box_x = planning_scene_box['box_x_dimension']
	box_y = planning_scene_box['box_y_dimension']
	box_z = planning_scene_box['box_z_dimension']
	table_pose.pose.position.x = planning_scene_box['box_position_x']
	table_pose.pose.position.y = planning_scene_box['box_position_y']
	table_pose.pose.position.z = planning_scene_box['box_position_z']
	table_pose.pose.orientation.x = planning_scene_box['box_orientation_x']
	table_pose.pose.orientation.y = planning_scene_box['box_orientation_y']
	table_pose.pose.orientation.z = planning_scene_box['box_orientation_z']
	table_pose.pose.orientation.w = planning_scene_box['box_orientation_w']
	self.scene.add_box(planning_scene_box['scene_name'], table_pose, (box_x, box_y, box_z))

I think it would be best to this as an initialization step before any action, as this only requires iterating over the yaml file with respective information.

This would also allow us to publish the static transforms for the objects once and not in each place action:

grasplan/src/grasplan/place.py

Lines 274 to 275 in f628097

	# TODO: find a better place for adding the static transforms, it shouldn't be done for each place
	self.add_planning_scene_objects(self.scene)

cc @oscar-lima

rename rqt_grasplan to rqt grasp recorder or similar

Move planning scene utilities into separate repository

The planning scenes are a requirement for grasplan due to MoveIt!. To continue the clean up of this repository, I would propose to create another repository with all your utilities to create new planning scenes (would mainly include the rqt_gui from @oscar-lima). I could imagine something like planning_scene_helpers.

Grasplan placing only works with axis aligned planning scenes

While setting up mobipick_labs in another part of our building, we noticed that Grasplan or more specifically the generation of place poses only works for axis aligned planning scenes. @mintar came up with a quick fix for the demo (see ai-demo-branch) which involves generating the places poses in object coordinates (e.g from a table) and then transforming to map coordinates via a statically published transformation for earch object (see here). In the long run we should fix this properly.

I'm willing to work on this, but probably need some guidance on how to best approach this.

cc @oscar-lima

Calculate offset to reduce the area for safe placing

Currently the offset for reducing the place area (e.g top table surface) to a safe sub-area is hard coded:
https://github.com/aprilprojecteu/grasplan/blob/c8a27e6fa6685c20a441b1e61b4aeb7fc40634b1/src/grasplan/place.py#L205

We often need to adjust it for demos, further it also highly depends on the current object in the gripper and computing it based on that shouldn't be to difficult. However the different "upwards" axis described in #10 could require some special cases.

Grasplan base module

I think we should create a base module that handles some common initialization for the place, pick and insert actions. For example each actions needs the interfaces to MoveIt!:

grasplan/src/grasplan/place.py

Lines 113 to 125 in f628097

	try:
	rospy.loginfo('waiting for move_group action server')
	moveit_commander.roscpp_initialize(sys.argv)
	self.robot = moveit_commander.RobotCommander()
	self.robot.arm.set_planning_time(self.planning_time)
	self.robot.arm.set_goal_tolerance(arm_goal_tolerance)
	self.scene = moveit_commander.PlanningSceneInterface()
	rospy.loginfo('found move_group action server')
	except RuntimeError:
	rospy.logfatal(
	'grasplan place server could not connect to Moveit in time, exiting! \n' + traceback.format_exc()
	)
	rospy.signal_shutdown('fatal error')

grasplan/src/grasplan/pick.py

Lines 88 to 104 in f628097

	try:
	rospy.loginfo('waiting for move_group action server')
	moveit_commander.roscpp_initialize(sys.argv)
	self.robot = moveit_commander.RobotCommander()
	self.gripper = getattr(self.robot, gripper_group_name)
	self.robot.arm.set_planning_time(self.planning_time)
	self.robot.arm.set_goal_tolerance(arm_goal_tolerance)
	self.scene = moveit_commander.PlanningSceneInterface()
	rospy.loginfo('found move_group action server')
	except RuntimeError:
	# moveit_commander.roscpp_initialize overwrites the signal handler,
	# so if a RuntimeError occurs here, we have to manually call
	# signal_shutdown() in order for the node to properly exit.
	rospy.logfatal(
	'grasplan pick server could not connect to Moveit in time, exiting! \n' + traceback.format_exc()
	)
	rospy.signal_shutdown('fatal error')

a tf client etc, ....

Removing tables_demo_bringup as exec depend

By having a dependency on the tables_demo_bringup package a circular dependency is introduced into the mobipick_labs environment.

Currently it is only used to get the related planning scene yaml files, in the following locations:

launch/planning_scene/planning_scene_visualization.launch:5:  <arg name="yaml_path_to_read" default="$(find tables_demo_bringup)/config/$(arg world_config)_planning_scene.yaml" />
launch/planning_scene/find_planning_scene_boxes_helper.launch:7:  <arg name="yaml_path_to_read" default="$(find tables_demo_bringup)/config/$(arg world_config)_planning_scene.yaml" />
launch/planning_scene/find_planning_scene_boxes_helper.launch:8:  <arg name="yaml_path_to_write" default="$(find tables_demo_bringup)/config/$(arg world_config)_planning_scene.yaml" />

output of grep -rn -e "tables_demo_bringup"

@oscar-lima any idea on how should approach this?

place: investigate why for some objects the required dimension is 1 and for some others is 2

see: c8a27e6

is it because the power drill mesh is transformed? I think I remember that when recording grasps this particular object also gave trouble...

pre-check collisions in place poses before sending them to moveit

add FCL support to Grasplan, to discard place poses that are in collision with MoveIt planning scene objects.

The assumption is that FCL can check unfeasible solutions early on given objects in the planning scene and the sampled place pose. The current brute force sampling approach puts too much stress on MoveIt place pipeline by checking for an entire manipulation plan from scratch, maybe they already do that which would be smart but I don't know...

Use MoveIt .scene file instead of custom yaml file to persist a planning scene

MoveIt provides a .scene format to persist and load planning scenes. Replacing our custom yaml format to this format would allow easier integration of external planning scenes. The example below shows the structure of the file format for storing a box:

* table_1
13.22 2.15 0.36
0 0 0.707107 0.707107
1
box
1.4 0.7 0.72
0 0 0
0 0 0 1
0 0 0 0
0

instead of

- scene_name: 'table_1'
  frame_id: 'map'
  box_x_dimension: 1.40
  box_y_dimension: 0.70
  box_z_dimension: 0.72
  box_position_x: 13.22
  box_position_y: 2.15
  box_position_z: 0.36
  box_orientation_x: 0.0
  box_orientation_y: 0.0
  box_orientation_z: 0.7071
  box_orientation_w: 0.7071

Further there is also a node that can publish the scene from the .scene file https://github.com/ros-planning/moveit/blob/master/moveit_ros/planning/planning_components_tools/src/publish_scene_from_text.cpp, so that we don't have to parse and publish it (as it is currently done with the yaml files).

keep the object_recognition_mockup.py?

Migrate grasplan documentation?

Currently lives under:

https://github.com/aprilprojecteu/grasplan_documentation

Should we migrate it to:

https://github.com/DFKI-NI/grasplan_documentation

just like we did with grasplan repo.