Path constraints

[Marcus Liebhardt]

Hi everyone!

I just started to test MoveIt's path constraint feature, but the resulting performance is very low, i.e. very few successful motion planning attempts.

I'm trying to keep the end effector parallel to the ground. To implement this I chose to add a orientation constraint. For my robot only one axis needs to be constrained, since the other two are already constrained by the goal position and the robot's kinematics.

Setting my path constraint looks like this:

    motion_plan_request.goal_constraints.append(goal_constraint)

    wrist_path_constraint = moveit_msgs.Constraints()

    wrist_path_constraint.name = "fixed wrist orientation"

    wrist_orientation_constraint = moveit_msgs.OrientationConstraint()

    wrist_orientation_constraint.header = userdata.goal_pose.header

    wrist_orientation_constraint.link_name = "palm_link"

    wrist_orientation_constraint.orientation = orientation_constraint.orientation # setting goal orientation

    wrist_orientation_constraint.absolute_x_axis_tolerance = 3.14 # ignore errors in the x-axis

    wrist_orientation_constraint.absolute_y_axis_tolerance = 3.14 # ignore errors in the y-axis

    wrist_orientation_constraint.absolute_z_axis_tolerance = 0.08 # enforce z-axis orientation

    wrist_orientation_constraint.weight = 1.0

    wrist_path_constraint.orientation_constraints.append(wrist_orientation_constraint)

    motion_plan_request.path_constraints = wrist_path_constraint

Although I increased the allowed planning time significantly (2.0 secs -> 120 secs), my success rate is very low, i.e. 1 out of 4 attempts. I also tried the two planners BKPIECEkConfigDefault and SBLkConfigDefault without any better results. Furthermore, if a valid motion plan is returned it contains a lot of useless motions (e.g. moving forward and backward a couple of times). I'm using the default KDL solver.

Am I missing any parameters or other settings to improve this performance?

Best,

Marcus

[Ioan Sucan]

Hello Marcus,

The low performance is unfortunately no surprise. When using path constraints IK calls are made for every state sampled. KDL is pretty slow (compared to an analytical solver) and planning with path constraints needs a lot of IK calls. Ideally, you should use an analytical solver in this case. Because of this problem the path simplification algorithms do not perform well -- not enough time is available for simplification. That can be fixed by changing the amount of time that path simplification algorithms are run for. Probably how the time is split between the different path shortening procedures should also be changed.

I have had better success with the RRTConnectkConfigDefault planner configuration when planning with constraints.

Ioan

[Tobias Fromm]

Hi guys,

as I am working on the same task right now, here's some more feedback on
the topic for people who have similar issues: Keeping the EEF parallel
to the ground works well for me with the mentioned settings.
I'm using RRTConnectkConfigDefault and x/y axis tolerances of 0.1; lower
tolerances gave bad results in terms of planning success rates.

Thanks for the background info, Ioan!
Cheers,
Tobi

--
Tobias Fromm
Robotics Group
Jacobs University

Research I, Room 38 +49-421-200-3106
Campus Ring 1 t.f...@jacobs-university.de
28759 Bremen, Germany http://robotics.jacobs-university.de

[Daniel Solomon]

From my understanding, supported by the comment from Tobias Fromm below, the tolerances on orientation are measured around an axis. From your constraint definition, it looks like you are trying to limit the angle of the tool z-axis to the world z-axis, but this is not how the constraint is interpreted. Try it with

    wrist_orientation_constraint.absolute_x_axis_tolerance = 0.08 # ignore errors in the x-axis

    wrist_orientation_constraint.absolute_y_axis_tolerance = 0.08 # ignore errors in the y-axis

    wrist_orientation_constraint.absolute_z_axis_tolerance = 3.14 # enforce z-axis orientation

[Tobias Fromm]

Hi Daniel,

right, I completely filtered out that Marcus might have had a flaw in
the point. Thanks for the clarification!
Your method is definitely right and I'm using it the same way
implicitly, although I was too vague in my description.

Cheers,
Tobi

On 11/08/2013 09:42 PM, Daniel Solomon wrote:
> From my understanding, supported by the comment from Tobias Fromm below,

> the tolerances on orientation are measured *around* an axis. From your

[Johannes P]

Hi guys

this thread is rather old but i have some problems with path constraints...
I want hold the EEF e.g. parallel to the ground while moving to a pose goal (with a custom 6DOF arm +gripper). Without path constraints MoveIt! generates a smooth trajectory.
But if i use orientation constraints, the default planner can't find a trajectory. Also the recommended RRTConnectkConfigDefault doesn't generate usable trajectories. There is always a jump/gap between two way points in the mid of the planned path. It seems, that the trajectory is planned from two sides (goal and start state) and both parts doesn’t fit in the mid. I also filled in 3.14 as constraint for each axis (= no constraints) but got the same unusable trajectories...

here is the code:
    move_group_interface::MoveGroup arm_group("arm");
   
    arm_group.setPlannerId("RRTConnectkConfigDefault");
    arm_group.setPlanningTime(45);

    //PATH CONSTRAINTS
    orientation_constraints_.link_name = reference_frame;
    orientation_constraints_.header.frame_id = "arm_base_link";
    //goal orientation
    orientation_constraints_.orientation.x = pose_target.orientation.x;
    orientation_constraints_.orientation.y = pose_target.orientation.y;
    orientation_constraints_.orientation.z = pose_target.orientation.z;
    orientation_constraints_.orientation.w = pose_target.orientation.w;
    //constraints
    orientation_constraints_.absolute_x_axis_tolerance = 3.14;
    orientation_constraints_.absolute_y_axis_tolerance = 3.14;
    orientation_constraints_.absolute_z_axis_tolerance = 3.14;
 
    constraints.orientation_constraints.push_back(orientation_constraints_);
    arm_group.setPathConstraints(constraints);
   
    arm_group.setStartStateToCurrentState();
    arm_group.setPoseTarget(pose_target, reference_frame);
    arm_group.move();

[Chris Reardon]

Hi Johannes,

I am having the exact same problem.  Have you been able to resolve it?

Thanks,
Chris

[Johannes P]

Hi Chris,

I did not deal with the path constraints further, because i don't exactly know how to solve the problem and had the hope, that someone could give me a hint.
But you could try the IKFast-Plugin, maybe it generates better trajectories...

[Chris Reardon]

Thanks Johannes for your reply. I will try a different plugin; however, from what I understood 3.14 should basically be equivalent to no constraint, and for me it was failing to find a solution no matter how long of a time limit I set. So I figured I was doing something wrong when configuring the constraint. 

It seems a simple enough concept. I'd really like to know how to do it correctly!  

[Johannes P]

Hi Chris,

I also wondering, why planning with path constraints and x,y,z=3.14 fails, since it should be equivalent to no constraints (as you said). But i also can't find mistakes in my code (from this tutorial)...
Another idea is the adjustment of the time used for path simplification (after it found a trajectory). Unfortunately i don't know how/where the parameter is set.

At the end I am as far as you with path constraints.
Maybe someone with a working path constraint implementation can point us in the right direction...

[Patrick Goebel]

Back to this older thread...

I am using the latest Hydro Debs for MoveIt and I can get basic path constraints to work in Python using the KDL IK solver but it is quite slow as others have pointed out (and Ioan explained why).  However, when I try my custom IKFast plugin instead of KDL (which works fine for unconstrained target poses), I get the following error:

[ INFO] [1405383421.370711860]: Planning attempt 1 of at most 5
[ INFO] [1405383421.380575364]: right_arm_shoulder_pan_joint -1.57 1.57 1
[ INFO] [1405383421.381511196]: right_arm_shoulder_lift_joint -3.14 3.14 1
[ INFO] [1405383421.381707894]: right_arm_shoulder_roll_joint -3.14 3.14 1
[ INFO] [1405383421.381874023]: right_arm_elbow_flex_joint -1.75 1.75 1
[ INFO] [1405383421.382037786]: right_arm_forearm_flex_joint -1.75 1.75 1
[ INFO] [1405383421.382148723]: right_arm_wrist_flex_joint -1.75 1.75 1
[ERROR] [1405383421.383264477]: Can only compute FK for IKTYPE_TRANSFORM_6D!
[ERROR] [1405383421.383489373]: Can only compute FK for IKTYPE_TRANSFORM_6D!
[ INFO] [1405383421.383629399]: No planner specified. Using default.
[ WARN] [1405383421.383803001]: RRTConnect: Skipping invalid start state (invalid state)
[ERROR] [1405383421.383966596]: RRTConnect: Motion planning start tree could not be initialized!
[ INFO] [1405383421.384114323]: No solution found after 0.000316 seconds
[ WARN] [1405383421.393454342]: Goal sampling thread never did any work.
[ INFO] [1405383421.393875509]: Unable to solve the planning problem

Is this to be expected?  Here is my code snippet where the problem arises:

    # Create a contraints list and give it a name
    constraints = Constraints()
    constraints.name = "Gripper horizontal"
   
    # Create an orientation constraint for the right gripper
    orientation_constraint = OrientationConstraint()
    orientation_constraint.header = start_pose.header
    orientation_constraint.link_name = right_arm.get_end_effector_link()
    orientation_constraint.orientation.w = 1.0
    orientation_constraint.absolute_x_axis_tolerance = 0.1
    orientation_constraint.absolute_y_axis_tolerance = 0.1
    orientation_constraint.absolute_z_axis_tolerance = 3.14
    orientation_constraint.weight = 1.0
   
    # Append the constraint to the list of contraints
    constraints.orientation_constraints.append(orientation_constraint)
     
    # Set the path constraints on the right_arm
    right_arm.set_path_constraints(constraints)
   
    # Set a target pose for the arm       
    target_pose = PoseStamped()
    target_pose.header.frame_id = REFERENCE_FRAME
    target_pose.pose.position.x = 0.173187824708
    target_pose.pose.position.y = -0.0159929871606
    target_pose.pose.position.z = 0.692596608605
    target_pose.pose.orientation.w = 1.0

    # Set the start state and target pose, then plan and execute
    right_arm.set_start_state_to_current_state()
    right_arm.set_pose_target(target_pose, end_effector_link)
    right_arm.go()
    rospy.sleep(1)
         

Thanks!
patrick

On 05/08/2014 01:15 AM, Johannes P wrote:

Hi Chris,

I did not deal with the path constraints further, because i don't exactly 
know how to solve the problem and had the hope, that someone could give me 
a hint.
But you could try the IKFast-Plugin<http://docs.ros.org/hydro/api/moveit_ikfast/html/doc/ikfast_tutorial.html>, 
maybe it generates better trajectories...

[Tobias Fromm]

Hi Patrick,

I guess the issue is with

> [ERROR] [1405383421.383264477]: Can only compute FK for IKTYPE_TRANSFORM_6D!

which, I guess, basically says that IKFast works work 6-DOF arms only.
Anyway, I remembered that there was an issue with this IKTYPE_TRANSFORM_6D definition a while ago which was fixed, but
this #ifndef must have been forgotten. So, as IKTYPE_TRANSFORM_6D is not defined, this error will pop up regardless of
the number of DOFs on your arm.

If you have a 6-DOF arm, I guess that IKFast should basically be working. Try locating and commenting the whole "#ifndef
IKTYPE_TRANSFORM_6D" block in IKFastKinematicsPlugin::getPositionFK(..) in your
blah_moveit_ikfast_plugin/blah_ikfast_moveit_plugin.cpp and re-compile this plugin, on a 6-DOF arm it should work then.

I've also filed an issue on moveit_ikfast on this: https://github.com/ros-planning/moveit_ikfast/issues/32

Cheers,
Tobi

[Patrick Goebel]

Thanks Tobias--that was it exactly. I do have a 6-DOF arm and
commenting out the block you suggested and rebuilding fixed the
problem. Now planning with constraints is *much* faster than when using
the KDL plugin.

--patrick

[Sam Lin]

Hi everyone!

I am also trying to use this path constraint feature and I am also experiencing some problems( same as Johannes). I am also trying keep the end effector parallel to the ground moving from point A to point B.
My problem is that the motion planner (using RRTConnectkConfigDefault) says its found a solution but when I display the trajectory it has found in rviz it shows a trajectory that looks invalid. I am using the UR10 arm (6 DOF) with default KDL kinematic solver.

From the trajectory it shows the end effector does maintain parallel (no tilting in any axis), however the other joints sometimes "skip/jump". By that I mean for example the elbow joint could be above the end effector while moving then suddenly its changed to being below the end effector than continues on to move to the goal position. Or the base joint would suddenly change 180 degrees in rotation in the middle of the movement. It seems like it just teleported from one configuration to the other, and the trajectory just doesn't seem like it would be valid for a real arm to be executed.

I was wondering if anyone else has also experienced this and could help me out.

Thank you!

Sam Lin

[Wouter van Oijen]

Hi Sam,

I've noticed such jumps too, when I was checking the effect of the "limited:=true" parameter on the command-line for the UR5 driver and/or moveit (as described in the README of the universal_driver package).
I've seen this behavior once when the driver was started with limited:=false and moveit was started with limited:=true, although that could have been coincidence. Maybe you have forgotten to supply this parameter which caused the invalid paths? I hope this helps you in the right direction.

NB: I'm also having a lot of trouble with the motion planning for the UR5, and I'm not even using path constraints. I've recently asked a question about this on ROS Answers:
http://answers.ros.org/question/194937/moveit-commander-not-able-to-find-a-motion-plan/
I'm still very interested in other users' experiences with motion planning for the UR5 (or UR10), since I'm a bit stuck here...

Best regards,
Wouter

[石升]

Hello Lin,

         I am using UR10 arm with default KDL kinematic solver, too and i also try to constraint the end effector parallel to the ground and keep it horizontal. But the planning result is 

 

Fail: ABORTED: No motion plan found. No execution attempted.

My code is shown as followed and i am sure the pose 'next_pose" is reachable. Would you mind checking the code and pointing out the mistake?

int main(int argc, char **argv)
{
   ros::init(argc, argv, "move_group_interface_tutorial");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();




    /* This sleep is ONLY to allow Rviz to come up */
    sleep(20.0);


    moveit::planning_interface::MoveGroup group_r("arm");
    group_r.setPlannerId("RRTConnectkConfigDefault");
    group_r.setPlanningTime(45);
//ocm
    moveit_msgs::OrientationConstraint ocm;


    ocm.link_name = "tt_base_link";
    ocm.header.frame_id = "base_link";










    ocm.orientation.x = 0.210738;
    ocm.orientation.y = 0.672778;
    ocm.orientation.z = 0.677635;
    ocm.orientation.w = 0.209212;


    ocm.absolute_x_axis_tolerance = 0.01;
    ocm.absolute_y_axis_tolerance = 0.01;
    ocm.absolute_z_axis_tolerance = 3.14;
    ocm.weight = 1.0;
    moveit_msgs::Constraints test_constraints;
    test_constraints.orientation_constraints.push_back(ocm);
    group_r.setPathConstraints(test_constraints);


//start pose


   
   geometry_msgs::Pose start_pose;
    start_pose.position.x = -0.402141;
    start_pose.position.y = 0.162843;
    start_pose.position.z = 0.384870;
    start_pose.orientation.x = -0.003270;
    start_pose.orientation.y = 0.704558;
    start_pose.orientation.z = 0.709631;
    start_pose.orientation.w = -0.003249;
   


    const robot_state::JointModelGroup *joint_model_group =
                 //start_state.getJointModelGroup(group_r.getName());
 start_state.setFromIK(joint_model_group, start_pose);
    group_r.setStartState(start_state);


// target  
    geometry_msgs::Pose next_pose;
 


       
        next_pose.position.x =-0.402141;
        next_pose.position.y =0.462843;
        next_pose.position.z =0.384870;
        next_pose.orientation.x =0.167744;
        next_pose.orientation.y =0.684582;
        next_pose.orientation.z =0.689516;
        next_pose.orientation.w =0.166666;


        group_r.setPoseTarget(next_pose,"tt_base_link");




 moveit::planning_interface::MoveGroup::Plan my_plan;
 bool success = group_r.plan(my_plan);


 ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");


 
        sleep(5.0);
 group_r.clearPathConstraints();
}

在 2014年10月20日星期一 UTC+8上午10:06:18，Sam Lin写道：