def pose_constraint_to_position_orientation_constraints(pose_constraint):
position_constraint = PositionConstraint()
orientation_constraint = OrientationConstraint()
position_constraint.header = pose_constraint.header
position_constraint.link_name = pose_constraint.link_name
position_constraint.position = pose_constraint.pose.position
position_constraint.constraint_region_shape.type = geometric_shapes_msgs.msg.Shape.BOX
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x)
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y)
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z)
position_constraint.constraint_region_orientation.x = 0.0
position_constraint.constraint_region_orientation.y = 0.0
position_constraint.constraint_region_orientation.z = 0.0
position_constraint.constraint_region_orientation.w = 1.0
position_constraint.weight = 1.0
orientation_constraint.header = pose_constraint.header
orientation_constraint.link_name = pose_constraint.link_name
orientation_constraint.orientation = pose_constraint.pose.orientation
orientation_constraint.type = pose_constraint.orientation_constraint_type
orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
orientation_constraint.weight = 1.0
return position_constraint, orientation_constraint
def pose_constraint_to_position_orientation_constraints(pose_constraint):
position_constraint = PositionConstraint()
orientation_constraint = OrientationConstraint()
position_constraint.header = pose_constraint.header
position_constraint.link_name = pose_constraint.link_name
position_constraint.position = pose_constraint.pose.position
position_constraint.constraint_region_shape.type = geometric_shapes_msgs.msg.Shape.BOX
position_constraint.constraint_region_shape.dimensions.append(
2 * pose_constraint.absolute_position_tolerance.x)
position_constraint.constraint_region_shape.dimensions.append(
2 * pose_constraint.absolute_position_tolerance.y)
position_constraint.constraint_region_shape.dimensions.append(
2 * pose_constraint.absolute_position_tolerance.z)
position_constraint.constraint_region_orientation.x = 0.0
position_constraint.constraint_region_orientation.y = 0.0
position_constraint.constraint_region_orientation.z = 0.0
position_constraint.constraint_region_orientation.w = 1.0
position_constraint.weight = 1.0
orientation_constraint.header = pose_constraint.header
orientation_constraint.link_name = pose_constraint.link_name
orientation_constraint.orientation = pose_constraint.pose.orientation
orientation_constraint.type = pose_constraint.orientation_constraint_type
orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
orientation_constraint.weight = 1.0
return position_constraint, orientation_constraint
def move_arm_constrained(self,
start_angles=None,
location=None,
blocking=1):
current_pose = self.get_current_wrist_pose_stamped('base_link')
move_arm_goal = self.create_move_arm_goal()
# default search-starting arm angles are arm-to-the-side
if start_angles == None:
if self.whicharm == 'l':
start_angles = [
2.135, 0.803, 1.732, -1.905, 2.369, -1.680, 1.398
]
else:
start_angles = [
-2.135, 0.803, -1.732, -1.905, -2.369, -1.680, 1.398
]
# default location is arm-to-the-side
if location == None:
if self.whicharm == 'l': location = [0.05, 0.576, 0.794]
else: location = [0.05, -0.576, 0.794]
# add a position constraint for the goal
position_constraint = PositionConstraint()
position_constraint.header.frame_id = current_pose.header.frame_id
position_constraint.link_name = self.ik_utilities.link_name #r_ or l_wrist_roll_link
set_xyz(position_constraint.position, location)
position_constraint.constraint_region_shape.type = Shape.BOX
position_constraint.constraint_region_shape.dimensions = [0.02] * 3
position_constraint.constraint_region_orientation.w = 1.0
position_constraint.weight = 1.0
move_arm_goal.motion_plan_request.goal_constraints.position_constraints.append(
position_constraint)
# search for a feasible goal pose that gets our wrist to location while keeping all but the (base-link) yaw fixed
current_pose_mat = pose_to_mat(current_pose.pose)
current_pose_mat = pose_to_mat(current_pose.pose)
found_ik_solution = 0
for angle in range(0, 180, 5):
for dir in [1, -1]:
rot_mat = tf.transformations.rotation_matrix(
dir * angle / 180. * math.pi, [0, 0, 1])
rotated_pose_mat = rot_mat * current_pose_mat
desired_pose = stamp_pose(mat_to_pose(rotated_pose_mat),
'base_link')
desired_pose.pose.position = position_constraint.position
# check if there's a collision-free IK solution at the goal location with that orientation
(solution, error_code) = self.ik_utilities.run_ik(
desired_pose,
start_angles,
self.ik_utilities.link_name,
collision_aware=1)
if error_code == "SUCCESS":
found_ik_solution = 1
break
if found_ik_solution: break
else:
rospy.loginfo("no IK solution found for goal, aborting")
return 0
# add an orientation constraint for the goal
orientation_constraint = OrientationConstraint()
orientation_constraint.header.stamp = rospy.Time.now()
orientation_constraint.header.frame_id = current_pose.header.frame_id
orientation_constraint.link_name = self.ik_utilities.link_name
orientation_constraint.orientation = desired_pose.pose.orientation
orientation_constraint.type = OrientationConstraint.HEADER_FRAME
orientation_constraint.absolute_roll_tolerance = 0.2
orientation_constraint.absolute_pitch_tolerance = 0.5
orientation_constraint.absolute_yaw_tolerance = 2 * math.pi
orientation_constraint.weight = 1.0
move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints.append(
orientation_constraint)
# add an orientation constraint for the entire path to keep the object upright
path_orientation_constraint = copy.deepcopy(orientation_constraint)
# temporary, while deepcopy of rospy.Time is broken
path_orientation_constraint.header.stamp = rospy.Time(
orientation_constraint.header.stamp.secs)
path_orientation_constraint.orientation = current_pose.pose.orientation
move_arm_goal.motion_plan_request.path_constraints.orientation_constraints.append(
path_orientation_constraint)
# send the goal off to move arm, blocking if desired and modifying the start state if it's in collision
result = self.send_move_arm_goal(move_arm_goal, blocking)
return result
