Trajectory "jumps" when planning with orientation path constraints

[Junhong Liang]

Hello everyone!


So I have been doing some test with path constraints for the last couple of days. What it really bothers me is that when I plan with orientation path constraints, I always get trajectories which seems to "jump" at certain points. So I capture one of those trajectories and extracted two points, which seem to be the problem:

...
positions: [0.07030076982423171, -0.060087761165928635, -1.7623868458114238, -0.8055791796513985, 0.06632340776779573, 2.49202134613582]
velocities: [0.010108259866117523, -0.01800968406797939, 0.020192532227076853, -0.49630065376978627, -0.026644927582898645, -0.421714333938974]
accelerations: [-0.008078562597439061, 0.009811037477290657, -0.007694065363548413, 0.12815414524762112, 0.03344371346892065, -1.0059267414426358]
effort: []
time_from_start:
secs: 1
nsecs: 916516542
-
positions: [0.07474439076488598, -0.08461940243371537, -1.7228947510669521, -1.9972359537230413, 0.09865933075461127, -2.562606872755544]
velocities: [0.025603867301923242, -0.023606231187315568, 0.022925844088660524, -0.3980391520039673, 0.04631621435255396, -0.5156269910710267]
accelerations: [0.023976273459930215, -0.016057216871521184, 0.010957032514299437, -0.040468771594689944, 0.036180197223611675, 0.9867665299302922]
effort: []
time_from_start:
secs: 5
nsecs: 253830651
...


When you look at the positions between the two points, there are such big jumps in axis 6 (and maybe also in axis 4). The pose of the end effector, however, remains almost the same. It seems to me that it suddenly switch to another IK solution. What I don't understand is, why would such two points considered to be adjacent by the planner? I have never had such problem in planning WITHOUT orientation path constraints, even with the same start point and target pose.

The following is the code I use for planning. Did I miss something or doing something wrong? What the code trying to do is to move the end effector while grapping a box. The box can rotate around the y-axis, but it can't tilt too hard otherwise the things in the box will fall out. I am using RRTConnectkConfigDefault as planner and an IKFast solver for my Schunk LWA 4P arm.


// Setting the target pose
geometry_msgs::Pose target_pose1;
  target_pose1.orientation.x = -0.70710678;
  target_pose1.orientation.y = 0.0;
  target_pose1.orientation.z = 0.0;
  target_pose1.orientation.w = 0.70710678;
  target_pose1.position.x = 0.0;
  target_pose1.position.y = 0.5;
  target_pose1.position.z = 0.4;
  group.setPoseTarget(target_pose1);

// Setting the start joint state 
  robot_state::RobotState start_state(*group.getCurrentState());
  geometry_msgs::Pose start_pose2;
  start_pose2.orientation.x = 0.5;
  start_pose2.orientation.y = 0.5;
  start_pose2.orientation.z = -0.5;
  start_pose2.orientation.w = -0.5;
  start_pose2.position.x = -0.4;
  start_pose2.position.y = 0.0;
  start_pose2.position.z = 0.4;
  const robot_state::JointModelGroup *joint_model_group =
                  start_state.getJointModelGroup(group.getName());
  start_state.setFromIK(joint_model_group, start_pose2, 5, 1.0);

// Constructing orientation path constraints
moveit_msgs::OrientationConstraint ocm; 
  ocm.link_name = "gripper_palm_link"; 
  ocm.header.frame_id = "arm_base_link";
  ocm.orientation.x = 0.5;
  ocm.orientation.y = 0.5;
  ocm.orientation.z = -0.5;
  ocm.orientation.w = -0.5;
  ocm.absolute_x_axis_tolerance = 0.4;
  ocm.absolute_y_axis_tolerance = 2.5;
  ocm.absolute_z_axis_tolerance = 0.4;
  ocm.weight = 1.0;

// Adding path constraints
moveit_msgs::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm); 
  group.setPathConstraints(test_constraints);

// Start planning
  moveit::planning_interface::MoveGroup::Plan my_plan;
  group.setStartState(start_state);
  group.setPoseTarget(target_pose1);
  success = group.plan(my_plan);

Any helps will be appreciated! Thank you in advance!


Best regards,

Junhong Liang

[Jimmy Da Silva]

Hi robotics fellows,

I have exactly the same problem as Junhong. I am also using IKFast and pretty the same code for my Kuka LWR4+.
These jumps don't appear with non-constrained plans.

I join a short video to illustrate the problem.

Thank you for any help you may provide.

Kind regards,

Jimmy Da Silva

[Jorge Nicho]

Hello Junhong,

Have you try tweaking the "range" parameter for the RRTConnectkConfigDefault planner in the  ompl_planning.yaml file.  Smaller values tend to generate denser paths which may improve the resulting motion plan

In the file ompl_planning.yaml of the moveit config package you'll find the range parameter:

  RRTConnectkConfigDefault:

    type: geometric::RRTConnect

    range: 0.01

[Steffen P]

Yes smaller "range" can help but still is no reliable solution. Especially with the ik jumps making your arm go crazy. I am wondering if someone ever had this working. I miss something like the "jump threshold" like for cartesian paths.

[Jimmy Da Silva]

Hi all,

Just discovered an alternative to IKFast that seems to give really good results !
I am able to plan trajectories with orientation constraints with no IK jumps :)

https://bitbucket.org/traclabs/trac_ik/overview

Have fun !

Jimmy Da Silva

[rkeatin3]

Hi everyone,

I'm bumping this because I am experiencing this issue as well (even while using Trac-IK). You can see that the large jumps in joint space via the Rviz animation in the attached video. 

Any new suggested fixes? I'm currently using RRTConnectkConfigDefault, but I'm wondering if using a non-bidirectional search algorithm may be more robust.

Any input appreciated. Thanks!

-Ryan

[G.A. vd. Hoorn - 3ME]

On 01/05/2018 04:14 AM, rkeatin3 wrote:
> Hi everyone,
>
> I'm bumping this because I am experiencing this issue as well (even
> while using Trac-IK). You can see that the large jumps in joint space
> via the Rviz animation in the attached video.
>
> Any new suggested fixes? I'm currently using RRTConnectkConfigDefault,
> but I'm wondering if using a non-bidirectional search algorithm may be
> more robust.

The first thing to check would be that you have trac ik configured for
Distance (closest solution), instead of Speed (the first solution).

Check the trac ik documentation for more information on that.


Gijs

> to another IK solution. What I don't understand is, *why

> would such two points considered to be adjacent by the

> planner?* I have never had such problem in planning
> *WITHOUT* orientation path constraints, even with the

> --
> You received this message because you are subscribed to the Google
> Groups "MoveIt! Users" group.
> To unsubscribe from this group and stop receiving emails from it, send
> an email to moveit-users...@googlegroups.com
> <mailto:moveit-users...@googlegroups.com>.
> To view this discussion on the web visit
> https://groups.google.com/d/msgid/moveit-users/3e7cf013-84ed-4597-a569-cc229905e8d7%40googlegroups.com
> <https://groups.google.com/d/msgid/moveit-users/3e7cf013-84ed-4597-a569-cc229905e8d7%40googlegroups.com?utm_medium=email&utm_source=footer>.

[Patrick Beeson]

When I released the trac-IK plugin for moveit, it was unclear how people would use it. It appears that perhaps Distance should be the default now. I’m willing to take feedback on this.

[Ryan Keating]

Thanks for the tip, Gijs!

As a follow-up, I'm now confused about how kinematics parameters are loaded and propagated... 

Once my kinematics.yaml has been loaded, I can check that everything is as expected on the param server:

$for param in $(rosparam list | grep solve); do echo -n "$param=" && rosparam get $param; done

/robot_description_kinematics/post_oven/kinematics_solver=trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin

/robot_description_kinematics/post_oven/kinematics_solver_attempts=3

/robot_description_kinematics/post_oven/kinematics_solver_search_resolution=0.005

/robot_description_kinematics/post_oven/kinematics_solver_timeout=0.005

/robot_description_kinematics/post_oven/solve_type=Distance

/robot_description_kinematics/pre_oven/kinematics_solver=trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin

/robot_description_kinematics/pre_oven/kinematics_solver_attempts=3

/robot_description_kinematics/pre_oven/kinematics_solver_search_resolution=0.005

/robot_description_kinematics/pre_oven/kinematics_solver_timeout=0.005

/robot_description_kinematics/pre_oven/solve_type=Distance

All that looks correct, but in the startup dialog I also see the following:

[ INFO] [1515173510.897278725]: Looking in private handle: /move_group for param name: post_oven/position_only_ik

[ INFO] [1515173510.897764349]: Looking in private handle: /move_group for param name: post_oven/solve_type

[ INFO] [1515173510.898238086]: Using solve type Speed

I can obviously propagate those parameters into the /move_group namespace, but this isn't the behavior that I expected from the default planning_context.launch created by MoveIt Setup Assistant.

On Fri, Jan 5, 2018 at 9:02 AM, Patrick Beeson <bees...@gmail.com> wrote:

When I released the trac-IK plugin for moveit, it was unclear how people would use it.  It appears that perhaps Distance should be the default now.  I’m willing to take feedback on this.

--
You received this message because you are subscribed to a topic in the Google Groups "MoveIt! Users" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/moveit-users/_l6iYrzrrRc/unsubscribe.
To unsubscribe from this group and all its topics, send an email to moveit-users+unsubscribe@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/moveit-users/39e4d170-502d-4ca4-b39a-c25398c77d33%40googlegroups.com.

[Patrick Beeson]

https://github.com/ros-planning/moveit_ros/issues/629

To unsubscribe from this group and all its topics, send an email to moveit-users...@googlegroups.com.

[Ryan Keating]

Thanks, Patrick. It looks like some work has been done in resolving that issue in MoveIt Issue 701 and MoveIt Issue 714 on the new repo. Perhaps those changes need to be propagated into the Trac-IK plugin as in UniversalRobot Issue 334? 

https://github.com/ros-planning/moveit_ros/issues/629

To unsubscribe from this group and all its topics, send an email to moveit-users+unsubscribe@googlegroups.com.

[Ryan Keating]

Now that I've been able to resolve issues with loading of kinematics parameters, I'm not sure that I see a big improvement.  

Though I'm not sure how this would behave on an actual robot (as I've been using Gazebo for testing this functionality), simulation suggests that orientation constraints are currently a huge safety violation: the controllers happily interpolate in between those discontinuous waypoints (usually shoulder, elbow, and/or wrist flips), resulting in crazy paths with frequent collisions. 

[Patrick Beeson]

There's not much I can do about that.  My IK solver is agnostic to the software above.  I have my own trajectory shaping software that is independent of MoveIt!, so I don't use MoveIt! enough to help out further.

[Ryan Keating]

Agreed, Patrick; I was surprised to hear that changing IK parameters might help at all, but the actual issue here is obviously MoveIt allowing trajectories with large gaps. I think it's probably time to open an issue on github to get more traction on this.

On Thu, Jan 11, 2018 at 2:19 PM, Patrick Beeson <bees...@gmail.com> wrote:

There's not much I can do about that.  My IK solver is agnostic to the software above.  I have my own trajectory shaping software that is independent of MoveIt!, so I don't use MoveIt! enough to help out further.

--

You received this message because you are subscribed to a topic in the Google Groups "MoveIt! Users" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/moveit-users/_l6iYrzrrRc/unsubscribe.
To unsubscribe from this group and all its topics, send an email to moveit-users+unsubscribe@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/moveit-users/6629c365-81c6-4361-a89b-a3695b428558%40googlegroups.com.