orientation constraints and warehouse (python)

[Chris Reardon]

Greetings,

I'm new to MoveIt, so I apologize if this has been covered before.

I am looking for a way, using the MoveIt Commander python interface, to constrain the end effector, e.g. to a plane.  For imagination's sake, I'm trying to pick an object off the surface of a table, and I'd like to move the hand above it and close my fingers.  I don't really care how the hand is oriented for now, other than it's "flat".

I found this: http://moveit.ros.org/wiki/Warehouse in what appears to be the old site. It mentions a "Goal Constraint Query" > "Orientation constraint (rpy)", which sounds promising, but I can't find any more or any more on the new site (http://moveit.ros.org/?s=warehouse).

Is the warehouse the same as the constraints database?  If so, I see I can load a warehouse with MoveGroupCommander.set_constraints_database().  Can anyone provide me the format of the warehouse and how to set it up, load it, etc?

Thanks in advance,
Chris

[Emili Boronat]

Hi Chris,

I was willing to do the same a few weeks ago. I ended extending the moveit commander to be able to set the trajectory constrains directly from python code. Take a look at the code here.

To set constrains you can use the same call that was used also to specify the constrain ID to use from the database. Just pass to that function a  Constrain msg type. The behaviour of this call is 

the same that the call in cpp movegroup api.

I just noticed that I somehow forget to commit the changes in the documentation to make evident this feature. I will do that when add more bindinds for python to missing functionalities. Hope I will have

time the next week.

Hope it helps you!

Cheers

El divendres 28 de març de 2014 23:04:36 UTC+1, Chris Reardon va escriure:

[Chris Reardon]

Thanks for your reply Emili!  I will try to incorporate your changes and report back.

Thanks,
Chris

[Steffen P]

Emili's changes have been merged into hydro-devel.

A usage example:

constraints = Constraints()

constraints.name = "upright"

orientation_constraint = OrientationConstraint()

orientation_constraint.header = p.header

orientation_constraint.link_name = self.arm.get_end_effector_link()

orientation_constraint.orientation = p.pose.orientation

orientation_constraint.absolute_x_axis_tolerance = 0.5

orientation_constraint.absolute_y_axis_tolerance = 0.5

orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis

orientation_constraint.weight = 1

self.arm.set_path_constraints(constraints)

[Chris Reardon]

Thanks!  I'm currently running Ubuntu 12.04 with groovy, and noticing that a few things are missing.  Could you describe the work that would be involved in getting this functionality going in groovy?

Thanks,
Chris

[Emili Boronat]

Hi,

Yeah, this is not merged on groovy, but you can take a look at our forked repos. They still have the dev branches for this features. So you cant take a look an try to reply the cahnges on groovy-devel and see if it works properly. I have no groovy at the moment and I will not have time to install it and test them probably in this two next weeks. I think it should be prety straighforward and there is not much code added.

Bassically you need to extend the bindings for move_group in moveit_ros and then apply the proper wrappers or calls in moveit_commander package. The branches are easy to identify because i'm the last author there. (e1000i)

Hope this helps you a little. I'm on a rush now so don't had time for a better explanation!

Good luck! 

El dimarts 8 d’abril de 2014 21:01:46 UTC+2, Chris Reardon va escriure:

[Chris Reardon]

In looking at this the most noticeable difficulty seems to be the lack of deserializeMsg in groovy.  Before I go down that road, can you tell me if there is any method other than this change to set a path constraint?

Thanks,
Chris

[Patrick Goebel]

Hi Steffen and Emili,

I was trying out this new feature and ran into a C++ signature error. I
am running the latest debian MoveIt packages under Ubuntu 12.04.

First, I think there is a statement missing below just before the
set_path_constraint() statement, namely:

constraints.orientation_constraints.append(orientation_constraint)

So my code looks like this:

start_pose = right_arm.get_current_pose(end_effector_link)
constraints = Constraints()
constraints.name = "Upright"
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = right_arm.get_end_effector_link()
orientation_constraint.orientation = start_pose.pose.orientation

orientation_constraint.absolute_x_axis_tolerance = 0.5
orientation_constraint.absolute_y_axis_tolerance = 0.5
orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis
orientation_constraint.weight = 1

constraints.orientation_constraints.append(orientation_constraint)
right_arm.set_path_constraints(constraints)

But when the script gets to the last statement above I receive the error:

right_arm.set_path_constraints(constraints)
File
"/opt/ros/hydro/lib/python2.7/dist-packages/moveit_commander/move_group.py",
line 347, in set_path_constraints
if not self._g.set_path_constraints(value):
Boost.Python.ArgumentError: Python argument types in
MoveGroup.set_path_constraints(MoveGroup, Constraints)
did not match C++ signature:
set_path_constraints(moveit::planning_interface::MoveGroupWrapper
{lvalue}, std::string)
python:
/usr/include/boost/thread/pthread/pthread_mutex_scoped_lock.hpp:26:
boost::pthread::pthread_mutex_scoped_lock::pthread_mutex_scoped_lock(pthread_mutex_t*):
Assertion `!pthread_mutex_lock(m)' failed.
Aborted (core dumped)


Am I missing something?

Thanks!
patrick

[Patrick Goebel]

Thanks to a private message from Emili, my mistake was that I had
erroneously assumed that this new feature had already made it into the
Hydro debs. Now I'm running from source and it works without the
signature error.

--patrick