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, I set target pose in such way, that the first joint value is either -0.43 rad. or 2.71 rad. Of course without constraints it goes to -0.43 rad. In order to go to the target pose with the first joint at 2.71 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 value: 0.000000, tolerance_above: 2.890000, tolerance_below: 0.300000
[ INFO] [1518387484.328191166]: Position constraint satisfied on link 'end_point'. Desired: 0.000000, 0.000000, 1.300000, current: -0.000055, 0.000063, 1.300003
[ INFO] [1518387484.328288924]: Differences 5.52363e-05 -6.33628e-05 -3.49439e-06
[ INFO] [1518387484.328343911]: Orientation constraint satisfied for link 'end_point'. Quaternion desired: 0.000000 0.000000 0.000000 1.000000, quaternion actual: -0.000093 0.000454 0.000306 1.000000, error: x=0.000186, y=0.000908, z=0.000611, tolerance: x=0.001000, y=0.001000, z=0.001000
[ERROR] [1518387484.370899340]: lwa4p[RRTConnectkConfigDefault]: Unable to sample any valid states for goal tree
[ INFO] [1518389077.720587747]: lwa4p[RRTConnectkConfigDefault]: Created 1 states (1 start + 0 goal)
[ WARN] [1518389077.720745031]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.105370 seconds
[ERROR] [1518389077.721232082]: lwa4p[RRTConnectkConfigDefault]: Insufficient states in sampleable goal region
[ INFO] [1518389077.736597067]: Unable to solve the planning problem
[ INFO] [1518389079.739423379]: Execution request received for ExecuteTrajectory action.
[ WARN] [1518389079.739642992]: The trajectory to execute is empty
I don't get it, I thought the planner would find the second possible pose for reaching the target (first joint at 2.71 rad) since the -0.43 rad is unreachable because of the joint constraints.
I also tried set the joint value to 2.71 rad using joint space goal. It works well. Then I tried to set it to -0.43 rad. I expected the error above to show, instead the first joint goes to -0.10 rad (no idea why) and I got this output:
Attempted to merge incompatible constraints for joint 'arm_1_joint'. Discarding constraint.
Execution completed: SUCCEEDED
Could you please tell me what am I doing wrong, what I don't understand. I'm fairly new to ROS.
I'm using ROS Kinetic on Ubuntu 16.04, MoveIt! 0.9.9, planner RRTConnectkConfigDefault. I tried various kinematic solvers: custom generated IKFAST, KDL and also trac_ik.
Thank you for you help!
↧