Moveit path constraint not working in cartesian path planning

29 Views Asked by At

I want to apply a joint constraint to my cartesian path plan.

So far I have attempted two ways to set path constraints in cartesian path planning.

  1. I apply a constraint on the move group object in ROS
commander = moveit_commander.MoveGroupCommander(move_group_name, wait_for_servers=60.0)
joint_constraint = JointConstraint()
joint_constraint.joint_name = "arm/joint_4"  
joint_constraint.position = np.deg2rad(0)
joint_constraint.tolerance_above = 3.0
joint_constraint.tolerance_below = 3.0
joint_constraint.weight = 1.0
constraints = Constraints()
constraints.joint_constraints.append(joint_constraint)
commander.set_path_constraints(constraints)

  1. Since I am using the Cartesian path planning service to plan my cartesian path, I also tried to apply the constraint onto the GetCartesianPathRequest()
req = GetCartesianPathRequest()
if overwrited_start_state is None:
    req.start_state.is_diff = True
else:
    req.start_state = moveit_robot_state
req.group_name = move_group_name
req.header.frame_id = planning_ref_frame
req.header.stamp = rospy.Time.now()
req.waypoints = waypoints.poses
req.max_step = eef_step
req.jump_threshold = jump_threshold
# req.path_constraints = Constraints()
req.avoid_collisions=True
req.link_name = current_end_effector_link
req.cartesian_speed_limited_link = ""
req.max_cartesian_speed = 0.0
joint_constraint = JointConstraint()
joint_constraint.joint_name = "arm/joint_4"
joint_constraint.position = 0.00
joint_constraint.tolerance_above = 3.00
joint_constraint.tolerance_below = 3.00
joint_constraint.weight = 1.0

# Initialize the Constraints message and add the joint constraint
path_constraints = Constraints()
path_constraints.name = "Joint constraint"
path_constraints.joint_constraints.append(joint_constraint)

# Set the path_constraints in the request
req.path_constraints = path_constraints

Right now, if I don't use Cartesian path planning and focus on just achieve joint-space goals, the constraint seems to work. However, right now, using Cartesian path planning it does not respond to any constraint that I set in these two ways. I also tried setting parameters like enforce_joint_model_state_space to true but it does seem to do anything. Any help is appreciated. Thank you.

0

There are 0 best solutions below