egiljones's profile - activity

I'm wondering if there's a way to do the following in C++:

if(<my logger level is set to ROS_DEBUG>) 
{
  //execute a costly code block that I don't want to do for the sake of efficiency
  //unless my logger level is set to ROS_DEBUG.  
  ROS_DEBUG_STREAM("Some output based on the computation");
}

I need to do some computation to get useful output in DEBUG mode, but I don't want to do that computation in normal operation.

Change the odom_trans.header.frame_id to "base_link", and use a tf publisher to put it on the wire and things should work.

Publishing something called /world_joint is not what you need. The Wizard by default produces a joint called /world_joint that reads in data from TF, transforming between the frame listed under the multi_dof_joints entry for world_joint as the parent_frame_id in your planning_description.yaml and the one listed as child_frame_id (which must be the root of your urdf). This allows you to incorporate information from an odometric system. Thus in addition to publishing /joint_states for your joints, you need to have a TF system running that publishes a transform from parent_frame_id to child_frame_id - this can just be an identity transform, but it needs to exist or the planning system will never fully initialize.

Pretty much all the code I'm aware of that Willow Garage has produced in the arm navigation space is focused on getting the arm from Point A to Point B, generally in order to perform a manipulation operation. We allow users to impose constraints on the path - keep the end effector upright, for instance - but the path constraints are still designed to condition the path from point A to point B, not change the purpose of the planner.

Having the end effector follow a shape is a different kind of planning problem, one where the purpose is the path, not moving from one point to another, and we really don't have much code for such things. Coverage algorithm - for instance, plan a path that would paint an irregular surface as quickly as possible - are other examples of areas Willow Garage hasn't really focused on. There may be other code in the larger ROS landscape that does such things, but I'm not aware of it.

If I were to attempt to address such problems I'd look at the literature and try a few things out, using some of the lower-level tools from arm_navigation, like collision-checking and inverse kinematics. An approach that might work ok for attempting a geometric path would be to discretize the shape into a series of end effector positions, call IK for each one, and try to interpolate between the poses, making sure that the arm didn't hit joint limits or singularities. This might not give a great path, but could get the job done.

I suspect that general code that would do for coverage/path-focused problems what arm_navigation does for Point to Point travel would be a protracted research/tech-transfer project.

move_arm also calls SetPlanningSceneDiff, so if you want to supply a diff to it you need to set the planning_scene_diff field in the MoveArmGoal. You shouldn't actually need the other two calls to the environment server - just put the attached object in the MoveArmGoal.planning_scene_diff and you should get the desired behavior.

The only way you should have gotten kinematics_msgs in /opt/ros/fuerte/include is through installing ros-fuerte-moveit-msgs. It currently in the case that you can't really have this package installed and use arm_navigation at the same time. We'll try to figure out a way for these systems to co-exist, but for now I'd recommend de-installing moveit if you want to use arm_navigation.

I've used CHOMP with many arms with all revolute joints, and it should work with such arms. I've never used it with prismatic joints, but don't know of any reason why it shouldn't work. I bet you can at least get to point where the code doesn't crash with not much effort - it should be clear pretty quickly if things are working or not. I'm happy to accept a patch if you get things functional.

Or the third option would be that it may segfault. Looking at the code if there is an existing object of a given id and another in the diff the object in the diff will overwrite the previous object. Thus you should make sure that the diff contains a one-to-one relationship between shapes and poses.

Ah - now I understand. That's actually pretty straightforward - what I would do is combine the tutorials for adding a virtual object and checking state validity. Adding the virtual object in the planning_scene_diff portion of the planning scene message means that a planning scene will be returned to you that contains the new object. You then create a CollisionModels class and call setPlanningScene with the returned planning scene. The instance of CollisionModels will now contain the object at whatever position you've specified in the planning_scene_diff, and you can make collision checks for individual states or the entire trajectory as you like.

Note that you should use the service /environment_server/get_planning_scene as opposed to /environment_server/set_planning_scene_diff if you care about efficiency - the former will just return to you a merged planning scene, whereas the later will also forward the planning scene to all the other arm navigation components, which takes additional time.

You first call the get_pl

This failure occurs in the kinematics node when it can't convert the frame in the request into the frames that the ik solver requires. I would expect that the ik node is printing out a warning to rosconsole - what does that output say?

All the wizard does is produce configuration files, and if you can run the planning_components_visualizer then the problem lies in what the simulator is producing or the frames that you are setting for the header in the ik request. Without more information I can't really say what's going on.

I don't really understand what you are trying to do, but arm_navigation in no way supports objects that are moving at the same time the robot is. You'd need to do a great deal of coding to add that support. All the planning algorithms assume that the world is static for the robot. You can add a static object, and move that object around, but all the arm navigation components are going to plan based on the fact that the object is static.

You need wto things in place for the arm_navigation.launch file to work - first, you need to make sure that you have are publishing a joint state message with positions and velocities for every joint in your robot. Then you need to make sure that the controller is up and running, advertising the correct action (control_msgs/FollowJointTrajectoryAction), and that you have the correct action name set in the move_arm launch file.

You should be able to load the collada file directly into the wizard, as all it does is call the urdf parser with the file and the urdf parser supports collada. Specify the package location and path like you would for a urdf and post output if it fails to load.

The planning_scene_validity_server was put in place for users who still required a service call API for validity checks that were going to take a while - like checking the validity of a trajectory. Allowing these calls in the environment_server made it worse at maintaining the state of the system as the validity checks require locking underlying data structures. If you want to do validity checks from python using the validity server is your only real option. By design it only allows for validity checking of the last scene that was syncned and not necessarily the latest scene in the environment server.

For getting the robot state, you can still make service calls to the environment_server.

For validity checks if you are writing in C++ I would suggest fetching the planning scene from the environment server and making the checks using the C++ API for CollisionModels - this will be vastly more efficient than making repeated calls to the validity server.

In terms of system design, you should only need to setting the planning scene if you expect there to be new objects in scene (a collision map, published objects). If all you expect to happen is an updated robot state you can get that from the environment server and include it as part of the request you are making to an arm_navigation component like a planner or IK.

We realize that the system has gotten more complicated in some ways, but we believe the benefit in terms of efficiency, logging, and offline playback operation outweigh the complications.

Fix for both the type and the segfault released into arm_navigation 1.1.10 - it'll take a couple to get through the build farm and pushed into the public repos (it's already on ros-shadow-fixed for most distros). Let me know if you encounter more problems.

Actually, the environment server is perfectly happy to accept an empty collision map. If you build and start up this executable you should see that the environment server starts up:

#include <ros/ros.h>
#include <arm_navigation_msgs/CollisionMap.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "empty_collision_map_publisher");
  //figuring out whether robot_description has been remapped

  ros::NodeHandle nh;

  ros::Publisher pub = nh.advertise<arm_navigation_msgs::CollisionMap>("/collision_map_occ", 1, true);

  while(ros::ok()) {
    arm_navigation_msgs::CollisionMap coll;
    coll.header.frame_id = "odom_combined";
    coll.header.stamp = ros::Time::now();
    pub.publish(coll);
  }

  return 0;
}

The issue is that there are checks in collider.cpp to only publish if there are some occupied points (check out the first line of the publishCollisionMap function). One option is to add a parameter to collider that will cause it to publish even if there are no points - if this is acceptable I'll add it in and release arm_navigation_experimental.

The rule of thumb I'd use is if the object really exists and you'd like to include it in a number of subsequent queries then add it to the world using the collision_object topic. In reference to b.1., the planning_scene_validity_server should be broadcasting the latest planning_scene_information on a marker topic - the next time you start up the system try adding a marker topic and look for the one that involves "planning_scene".

I just released a fix - an intern added the option to use different trajectory filters, and renamed the parameter trajectory_filter_service_name to trajectory_filter_1_service_name, and I changed it back. You can either wait for debs to get built and show up or manually edit your launch file in the meantime.

I'll be releasing a fix into fuerte shortly...

You are correct that trying to pass in xacro is not working - the URDF parser will just ignore everything that's in xacro, which is why you are only getting your base link, as that's in urdf. To convert from xacro to urdf run the following command:

rosrun xacro xacro.py my_robot.xacro > my_robot.urdf

Replacing my_robot, of course, and then run the wizard with my_robot.urdf .

move_arm uses the control_msgs::FollowJointTrajectoryAction action to communicate with the arm controller. You can design the controller however you wish, as long as you expose an action server that takes this message and provides results when the actions are finished. The PR2 modules that expose this API are held in pr2_controllers/robot_mechanism_controllers as a reference. There has also been working writing such interfaces for industrial robots. For some references please see http://code.google.com/p/swri-ros-pkg/ .

move_arm uses the control_msgs::FollowJointTrajectoryAction action to communicate with the arm controller. You can design the controller however you wish, as long as you expose an action server that takes this message and provides results when the actions are finished. The PR2 modules that expose this API are held in pr2_controllers/robot_mechanism_controllers as a reference. There has also been working writing such interfaces for industrial robots. For some references please see http://code.google.com/p/swri-ros-pkg/

You shouldn't need to build arm_navigation from source - just apt-get install ros-fuerte-arm-navigation. If you do want to build from source on fuerte you'll need to install the separate ompl deb using 'sudo apt-get install ros-fuerte-ompl'. If you have other issues using ros-industrial code with fuerte update your question.

You shouldn't need to build arm_navigation from source - just apt-get install ros-fuerte-arm-navigation. If you do want to build from source on fuerte you'll need to install the separate ompl deb using 'sudo apt-get install ros-fuerte-ompl'. If you have other issues using ros-industrial code with fuerte

The mapping_rviz_plugin package is in the arm_navigation stack - did you try to 'sudo apt-get install ros-electric-arm-navigation'?

I can't speak to the KDL portion of the question, but as for the rest of arm navigation I think that things should function correctly even with a very simple arm. There is no requirement of having an IK solver at all - you'll lose out to some extent in terms of interactivity, and you won't be able to pass pose goals into the planners, but joint space goals should function correctly. All of the tools already contain interaction modes where you can set joint goals directly - just stick to these modes and I expect that you'll get plans returned.

As for the rest of it, it's pretty straight-forward to implement a new IK plugin - just start with the existing KDL plugin as a base, make a copy, assign it a new class name, and make whatever changes you want to try. Then just declare the plugin in the kinematics_plugin.xml file either in your own package or in a code checkout of arm_kinematics_constraint_aware (if doing it in your own package make sure there's a direct dependency on kinematics_base in the manifest.xml). Then change over your autogenerated launch files to point at the new plugin and you should be able to try things out using the tools.

The larger issue is that it may be difficult to pick things up with such a simple arm. If you have relatively few DOF in your arm it generally makes sense to compensate for that in terms of degrees of freedom of the base to allow you to reach more positions - if you have a holonomic base you can then get 5 DOF in terms of end effector position. Our tools don't really extend to utilizing this kind of base movement to expand the reachability of a lower DOF manipulator, and I imagine this will lessen the usefulness of arm navigation for your low DOF arm.