Menu
▾
▴
Re: [ompl-users] path constraints & strange arm movement
|
From: Sachin C. <sa...@wi...> - 2011-04-28 18:29:12
|
Hi Nic, Here are some answers to your questions: 1. For path constraints, as Ioan mentioned, the planners can take quite a while to converge. An alternative that you can use, depending on the geometry of your arm, is to plan in workspace coordinates and use inverse kinematics. We do that with the PR2 by planning in the end-effector cartesian degrees of freedom + the redundancy and it works pretty well. The advantage of planning in this space is that orientation constraints can be easily represented here. You can see how this planner is configured by checking this configuration file: pr2_arm_navigation_planning/config/ompl_planning.yaml. The file contains configuration for "right_arm_cartesian" and "left_arm_cartesian" planners which plan in end-effector + redundancy space for the PR2 arms. You can address these planners directly in that space - i.e. you can provide goals in end-effector+redundancy space. 2. For path smoothing, you can use the trajectory filters we have setup in ROS. They basically use a random short-cutting with cubic splines. The configuration for these filters is setup in pr2_arm_navigation_filtering. They should reduce the amount of such "randomized" motions that you see from the arms. Please note that some of these components are not well-documented yet but we are making a big effort to add more documentation and tutorials. Hope this helps, Best Regards, Sachin On Thu, Apr 28, 2011 at 7:08 AM, <is...@gm...> wrote: > Hello Nic, > > I am cc'ing Sachin Chitta (from Willow Garage), in case he has additional > comments. Responses are inline. > > On Apr 28, 2011 7:20am, "Dirkx, N.J." <n.j...@st...> wrote: >> Hi there >> , >> >> >> I am using OMPL planners for a 7DOF robotic arm (non-pr2) with ROS. More >> specific, I have set it up according to the pr2_arm_navigation stack, so I >> am using the move_arm node and the ompl_ros_interface. >> >> >> >> The entire chain from perception to execution is working, I only ran into >> two problems: >> >> >> >> 1.Path constraints: >> >> As soon as I add path constraints on the orientation of the end >> effector the planners fail to find a path. It doesn't matter which planner I >> choose, they all show the same behaviour. Even if the constraints are >> set really large (say 2.5 rad), they don't succeed in finding a solution. >> >> The following code snippet shows how I added the path constraints: >> >> >> >> //set path constraints >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints.resize(1); >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].header.frame_id >> = "base_link"; >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].header.stamp >> = ros::Time::now(); >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].link_name >> = "hand_right"; >> >> >> >> geometry_msgs::Quaternion quat_msg = >> tf::createQuaternionMsgFromRollPitchYaw(0.0, 0.0, 0.0); >> >> >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].orientation.x >> = quat_msg.x; >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].orientation.y >> = quat_msg.y; >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].orientation.z >> = quat_msg.z; >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].orientation.w >> = quat_msg.w; >> >> >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].type >> = motion_planning_msgs::OrientationConstraint::HEADER_FRAME; >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].absolute_roll_tolerance >> = 1.5; >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].absolute_pitch_tolerance >> = 1.5; >> >> >> goalB.motion_plan_request.path_constraints.orientation_constraints[0].absolute_yaw_tolerance >> = 1.5; >> >> >> >> My question are: >> >> Is this the right way to give in path constraints? > > To me it seems the setup is correct, but it has changed a bit since I last > used it. Perhaps Sachin can comment if there is something wrong. > > From the behaviour you describe for the planners, it sounds like they are > unable to find valid states. Do you by any chance have ros::console > information from OMPL? It should appear in your log and should say things > like "No solution found after ... seconds" and > ".... created ... states". I'm curious about the number of states created by > the planners. If this is low (under 10) it means there is something wrong > with the evaluation of validity, and perhaps in your input. > > >> >> To what extent can the planners deal with path constraints? >> > > The planners rely on sampling the state space. If a constraint will reduce > the volume of the valid space to 0 within the 7-dimensional state space for > the robot, the methods currently employed by the planners will not work. If > you keep the volume above 0, the probability of finding a solution is > proportional to the ratio of the valid volume with respect to the total > volume. > >> Is there a planner that is recommended if I want to use path >> constraints? >> > It really depends on the problem you are solving. I don't think I can say > this with certainty. I would avoid lazy planners perhaps. So... if SBL or > LBKPIECE1 or LazyRRT appear to be slower, they probably will be consistently > slower. > My top choices would be KPIECE1 and RRTConnect. OMPL just got a new planner > (BKPIECE1), which I think will soon be available through ROS as well. That > will perhaps be a good choice as well. > >> >> >> >> >> 2.Large/strange arm movement >> >> The planned planned path to the end pose very seems to be very >> indirect/ inefficient. The arm moves through a large part of the joint space >> before ending up on the right spot, resulting in very exotic arm movements. >> All planners show similar behaviour. >> > This is due to the randomized nature of the algorithms. Adding constraints > typically makes matters worse, from this point of view :( > >> Is there a way to suppress these movements or to punish joint states >> that have a large distance to, say, the ideal path? >> > We just added some smoothing code in OMPL, but it is not yet part of ROS. > This will still not give you an optimal path, but it should produce more > natural looking paths. > > > Ioan > >> >> >> Thanks >> >> >> >> Nic >> >> >> ------------------------------------------------------------------------------ >> >> WhatsUp Gold - Download Free Network Management Software >> >> The most intuitive, comprehensive, and cost-effective network >> >> management toolset available today. Delivers lowest initial >> >> acquisition cost and overall TCO of any competing solution. >> >> http://p.sf.net/sfu/whatsupgold-sd >> >> _______________________________________________ >> >> ompl-users mailing list >> >> omp...@li... >> >> https://lists.sourceforge.net/lists/listinfo/ompl-users >> >> -- Sachin Chitta Research Scientist Willow Garage |