Ros moveit planner with constraints gives no solution result
Hi I am using ros kinetic on ubuntu 16.4, I am trying to get the kuka iiwa 14 robot to do a planar motion around obstacles at a certain height. I have set the height so that it will be reachable on that plane for all points I will set it to. The planner generates a path without constraints, however, the height will change as well as orientation in between points, it is also doesn't go through the path I want it to, and thus I am using constraints. after using constraints the planner kept giving me no solution messages (I am using rrtconnect, with 200 sec of timeout and on 10 max tries, using track_ik kinematics solver). Here is a snippet of my code, I am wondering if anyone knows what the issue could be?
waypoints = [] #empty waypoints list
for i in range(0, len(data), 6):
#pose goal set
pose_goal = group.get_current_pose("iiwa_link_ee")
pos_con = moveit_msgs.msg.PositionConstraint() #create position_constraint message
pos_con.header = pose_goal.header #copy current header
pos_con.link_name = "iiwa_link_ee" #specify end effector link
pos_con.target_point_offset.x = 0
pos_con.target_point_offset.y = 0
pos_con.target_point_offset.z = 0 #point is right on origin
shape = shape_msgs.msg.SolidPrimitive() #create primitives message
shape.type = 1 #cube
shape.dimensions = [0.95, 0.555, 0.01] #xyz of cube
shapePose = geometry_msgs.msg.Pose() #create Pose msg
shapePose.position.x = 0.0
shapePose.position.y = 0.265
shapePose.position.z = data[i+2] #have it at current z
BoundVol = moveit_msgs.msg.BoundingVolume() #create BoundingVolume message
BoundVol.primitives.append(shape) #add shape info
BoundVol.primitive_poses.append(shapePose) #add shape pose info
pos_con.constraint_region = BoundVol #add BoundVol to constraint_region
pos_con.weight = 1 #weight of 1
pose_goal.pose.position.x = data[i] #set goal position
pose_goal.pose.position.y = data[i+1]
pose_goal.pose.position.z = data[i+2]
q = quaternion_from_euler(np.deg2rad(data[i+3]), np.deg2rad(data[i+4]), np.deg2rad(data[i+5]))
pose_goal.pose.orientation = Quaternion(*q)
pos_con.header = pose_goal.header #same as goal header
#orientation constraint, maintains end effector to be vertical
ori_con = moveit_msgs.msg.OrientationConstraint() #create orientation_constraint
ori_con.link_name = "iiwa_link_ee" #for EE link
ori_con.absolute_x_axis_tolerance = 3.14 #ignore this axis for constraints#
ori_con.absolute_y_axis_tolerance = 0.1
ori_con.absolute_z_axis_tolerance = 0.1
ori_con.weight = 0.9 #set weight slightly less
ori_con.header = pose_goal.header
ori_con.orientation = Quaternion(*q)
#constraint message
constraint = moveit_msgs.msg.Constraints()
constraint.orientation_constraints.append(ori_con) #adding orientation constarint to overal constraint
constraint.position_constraints.append(pos_con)
group.set_path_constraints(constraint) #set path overall constraints
group.set_goal_orientation_tolerance(0.01)
group.set_start_state_to_current_state() #move to goal
group.set_pose_target(pose_goal.pose, "iiwa_link_ee") #move to goal with constraints
planner = group.plan()
planner_points = planner.joint_trajectory.points #obtain the joint poses generated along path
state = self.robot.get_current_state() #get current state of robot
wpose = group.get_current_pose().pose #get current position
for j in range(len(planner_points)):
state.joint_state.position = planner_points[j].positions #obtain joint state
FK = GetPositionFKRequest() #service to find position based on forward kinematics
FK.header.frame_id = "world"
FK.fk_link_names.append("iiwa_link_ee")
FK.robot_state = state #set FK ...