Joint constraint problem

edit

Hello, I'm working on Schunk LWA 4P robotic arm with 6 DOF.

I'm doing basic pick&place operations. Since the range of the first joint is (-2.89;2.89) there are two poses that satisfy the goal for picking an object. Let's say it's -0.43 rad and 2.71 rad. After picking I set orientation constraints for the end effector since I need it to be facing ground. Here comes the problem, sometimes planner chooses to use the value 2.71 for the first joint while picking. But that means that during placing after setting orientation constraints the first joint has to go from 2.71 rad to (for example) -2.84 rad in order to keep the end effector facing ground. That is almost one full revolution around z axis. It would be much more efficient to pick with the first joint at -0.43 rad, set constraints and place with the first joint at 0.3 rad (-2.84+PI).

I tried everything that came to my mind but I cannot make the robot behave the way I want. I tried putting the arm to "start position" with first joint value at 0 rad with down facing end effector, then setting orientation constraints, so it will be forced to pick at -0.43 rad and place at 0.3 rad. It works 3 out of 4 times, that one time the orientation constraints seem to be ignored and the arm performs pick at the 2.71 rad with end effector in forbidden orientation during movement to goal position. Placing also ignores constraints. This happens randomly for the same goal poses, sometimes it works fine, sometimes not. With this not-always-working "solution" the arm also has to pick with orientation constraints and that is not good for me. I would just really like to know, why are orientation constraints ignored sometimes...

The best solution would be to set joint constraints for the first joint. (-1.57;1.57) is all I need and I thought this would solve my problem. However this doesn't work at all. I tried a simple code, just to test the joint constraints functionality, I set target pose in such way, that the first joint value is either -0.43 rad. or 2.71 rad. Without constraints it goes to -0.43 rad. Let's say this time I want it to go to 2.71 rad. In order to do this I set the joint constraints like this:

moveit_msgs::JointConstraint jc;
jc.joint_name = "arm_1_joint";  
jc.position = 0.0;
jc.tolerance_above = 2.89;
jc.tolerance_below = 0.3;
jc.weight = 1.0; 
moveit_msgs::Constraints path_constraints;
path_constraints.joint_constraints.push_back(jc);
group.setPathConstraints(path_constraints);

The arm doesn't move and I get the following output:

More than 80% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler. Is the constrained sampler working correctly?
[ INFO] [1518387484.328126868]: Constraint violated:: Joint name: 'arm_1_joint', actual value: -0.430208, desired ...