Orientation constrained Inverse Kinematics in Moveit is not working on Baxter

edit

Moving from point A to point B, I want Baxter arm to stay approximately vertically downward throughout the trajectory. The trajectory ends near to the workspace limit, hence arm must be stretched in order to reach point B. Below is the constraint:

• The orientation need not be constant throughout the trajectory. In fact, it can change within specified limits

Before performing the experiment, I moved the arm from point A to B within desired orientation and recorded this trajectory (end-effector position and orientation) in CSV file. Later, I used this CSV file to calculate orientation limits.

This is how I calculate orientation limits:

import numpy as np
from numpy.linalg import norm
from tf.transformations import euler_from_quaternion

# ee_data (numpy 2D array) is end-effector
# trajectory consisting of position and orientation.
# q_traj is quaternion trajectory of end-effector
q_traj = ee_data[:, 4:8]
q_mean = q_traj.mean(axis=0)
# normalize for making it perfect quaternion
q_mean_norm = q_mean/norm(q_mean)
# convert each quaternion into euler 
eu_traj = np.apply_along_axis(euler_from_quaternion, axis=1, arr=q_traj)
eu_std = eu_traj.std(axis=0)

This is how limits have been assigned:

from geometry_msgs.msg import Quaternion
from moveit_msgs.msg import Constraints, OrientationConstraint

orientation_constraint = OrientationConstraint()
orientation_constraint.orientation = Quaternion(*q_mean_norm)
orientation_constraint.absolute_x_axis_tolerance = eu_std[0]
orientation_constraint.absolute_y_axis_tolerance = eu_std[1]
orientation_constraint.absolute_z_axis_tolerance = eu_std[2]
orientation_constraint.weight = 0.8
orientation_constraint.link_name = group.get_end_effector_link()
orientation_constraint.header.frame_id = group.get_planning_frame()

constraint = Constraints()
constraint.orientation_constraints.append(orientation_constraint)
group.set_path_constraints(constraint)

The robot is already kept at point A, so I only need to set the target position. Please see below:

# point B is last point of recorded trajectory
postion_target = ee_data[-1, 1:4].tolist()
group.set_position_target(postion_target)

plan = group.plan()
if plan.joint_trajectory.points:
    print 'Moving arm...'
    group.go(wait=True)
    print 'Reached target pose'
else:
    print 'ERROR: No IK solution found'

The IK solver is not able to return any solution and I always get ERROR: No IK solution found error in terminal. I printed orientation_constraint variable and it looked fine.

print orientation_constraint
header:
  seq: 0
  stamp:
    secs: 0
    nsecs:         0
  frame_id: /base
orientation:
  x: 0.0912257059583
  y: 0.915770149186
  z: -0.376106400769
  w: 0.107642369597
link_name: left_gripper
absolute_x_axis_tolerance: 0.238215788631
absolute_y_axis_tolerance: 0.277075213625
absolute_z_axis_tolerance: 0.753014745451
weight: 0.8