Ros moveit planner with constraints gives no solution result

asked 2021-06-01 04:08:49 -0600

Logan_Zhang gravatar image

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 ...
(more)
edit retag flag offensive close merge delete