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_right_arm(self, position, orientation, frame_id, waiting_time):
goal = MoveArmGoal()
goal.motion_plan_request.group_name = "right_arm"
goal.motion_plan_request.num_planning_attempts = 10
goal.motion_plan_request.planner_id = ""
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request.allowed_planning_time = rospy.Duration(waiting_time / 10.);
position_constraint = PositionConstraint()
position_constraint.header.frame_id = frame_id
position_constraint.link_name = "r_wrist_roll_link"
position_constraint.position.x = position[0]
position_constraint.position.y = position[1]
position_constraint.position.z = position[2]
position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX
tolerance = 2 * 0.02
position_constraint.constraint_region_shape.dimensions = [tolerance, tolerance, tolerance]
position_constraint.constraint_region_orientation.x = 0.
position_constraint.constraint_region_orientation.y = 0.
position_constraint.constraint_region_orientation.z = 0.
position_constraint.constraint_region_orientation.w = 1.
position_constraint.weight = 1.0
orientation_constraint = OrientationConstraint()
orientation_constraint.header.frame_id = frame_id
orientation_constraint.link_name = "r_wrist_roll_link"
# orientation_constraint.type = dunno!
orientation_constraint.orientation.x = orientation[0]
orientation_constraint.orientation.y = orientation[1]
orientation_constraint.orientation.z = orientation[2]
orientation_constraint.orientation.w = orientation[3]
orientation_constraint.absolute_roll_tolerance = 0.04
orientation_constraint.absolute_pitch_tolerance = 0.04
orientation_constraint.absolute_yaw_tolerance = 0.04
orientation_constraint.weight = 1.0
goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint)
goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint)
goal.disable_collision_monitoring = True
# rospy.loginfo("Goal: " + str(goal))
state = self.move_right_arm_client.send_goal_and_wait(goal, rospy.Duration(waiting_time))
if state == actionlib.GoalStatus.SUCCEEDED:
return True
else:
return False
pc.constraint_region_orientation.w = 1.0
goalA.motion_plan_request.goal_constraints.position_constraints.append(pc)
oc = OrientationConstraint()
oc.header.stamp = rospy.Time.now()
oc.header.frame_id = 'torso_lift_link'
oc.link_name = 'r_wrist_roll_link'
oc.orientation.x = 0.
oc.orientation.y = 0.
oc.orientation.z = 0.
oc.orientation.w = 1.
oc.absolute_roll_tolerance = 0.04
oc.absolute_pitch_tolerance = 0.04
oc.absolute_yaw_tolerance = 0.04
oc.weight = 1.
goalA.motion_plan_request.goal_constraints.orientation_constraints.append(oc)
move_arm.send_goal(goalA)
finished_within_time = move_arm.wait_for_result()
if not finished_within_time:
move_arm.cancel_goal()
rospy.logout('Timed out achieving goal A')
else:
state = move_arm.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.logout('Action finished with SUCCESS')
else:
rospy.logout('Action failed')
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
pc.constraint_region_orientation.w = 1.0
goalA.motion_plan_request.goal_constraints.position_constraints.append(pc)
oc = OrientationConstraint()
oc.header.stamp = rospy.Time.now()
oc.header.frame_id = 'torso_lift_link'
oc.link_name = 'r_wrist_roll_link'
oc.orientation.x = 0.
oc.orientation.y = 0.
oc.orientation.z = 0.
oc.orientation.w = 1.
oc.absolute_roll_tolerance = 0.04
oc.absolute_pitch_tolerance = 0.04
oc.absolute_yaw_tolerance = 0.04
oc.weight = 1.
goalA.motion_plan_request.goal_constraints.orientation_constraints.append(
oc)
move_arm.send_goal(goalA)
finished_within_time = move_arm.wait_for_result()
if not finished_within_time:
move_arm.cancel_goal()
rospy.logout('Timed out achieving goal A')
else:
state = move_arm.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.logout('Action finished with SUCCESS')
else:
