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
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
rospy.init_node('arm_cartesian_goal_sender')
move_arm = actionlib.SimpleActionClient('move_right_arm', MoveArmAction)
move_arm.wait_for_server()
rospy.logout('Connected to server')
goalA = MoveArmGoal()
goalA.motion_plan_request.group_name = 'right_arm'
goalA.motion_plan_request.num_planning_attempts = 1
goalA.motion_plan_request.planner_id = ''
goalA.planner_service_name = 'ompl_planning/plan_kinematic_path'
goalA.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
pc = PositionConstraint()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = 'torso_lift_link'
pc.link_name = 'r_wrist_roll_link'
pc.position.x = 0.75
pc.position.y = -0.188
pc.position.z = 0
pc.constraint_region_shape.type = Shape.BOX
pc.constraint_region_shape.dimensions = [0.02, 0.02, 0.02]
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()
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
if __name__ == '__main__':
rospy.init_node('arm_cartesian_goal_sender')
move_arm = actionlib.SimpleActionClient('move_right_arm', MoveArmAction)
move_arm.wait_for_server()
rospy.logout('Connected to server')
goalA = MoveArmGoal()
goalA.motion_plan_request.group_name = 'right_arm'
goalA.motion_plan_request.num_planning_attempts = 1
goalA.motion_plan_request.planner_id = ''
goalA.planner_service_name = 'ompl_planning/plan_kinematic_path'
goalA.motion_plan_request.allowed_planning_time = rospy.Duration(5.)
pc = PositionConstraint()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = 'torso_lift_link'
pc.link_name = 'r_wrist_roll_link'
pc.position.x = 0.75
pc.position.y = -0.188
pc.position.z = 0
pc.constraint_region_shape.type = Shape.BOX
pc.constraint_region_shape.dimensions = [0.02, 0.02, 0.02]
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()
def testMotionExecutionBuffer(self):
global padd_name
global extra_buffer
#too much trouble to read for now
allow_padd = .05 #rospy.get_param(padd_name)
motion_plan_request = MotionPlanRequest()
motion_plan_request.group_name = "right_arm"
motion_plan_request.num_planning_attempts = 1
motion_plan_request.allowed_planning_time = rospy.Duration(5.)
motion_plan_request.planner_id = ""
planner_service_name = "ompl_planning/plan_kinematic_path"
motion_plan_request.goal_constraints.position_constraints = [
PositionConstraint() for _ in range(1)
]
motion_plan_request.goal_constraints.position_constraints[
0].header.stamp = rospy.Time.now()
motion_plan_request.goal_constraints.position_constraints[
0].header.frame_id = "base_link"
motion_plan_request.goal_constraints.position_constraints[
0].link_name = "right_arm_hand_link"
motion_plan_request.goal_constraints.position_constraints[
0].position.x = .65
motion_plan_request.goal_constraints.position_constraints[
0].position.y = -0.55
motion_plan_request.goal_constraints.position_constraints[
0].position.z = .9
motion_plan_request.goal_constraints.position_constraints[
0].constraint_region_shape.type = Shape.BOX
motion_plan_request.goal_constraints.position_constraints[
0].constraint_region_shape.dimensions = [
float(.02) for _ in range(3)
]
motion_plan_request.goal_constraints.position_constraints[
0].constraint_region_orientation.w = 1.0
motion_plan_request.goal_constraints.position_constraints[
0].weight = 1.0
motion_plan_request.goal_constraints.orientation_constraints = [
OrientationConstraint() for _ in range(1)
]
motion_plan_request.goal_constraints.orientation_constraints[
0].header.stamp = rospy.Time.now()
motion_plan_request.goal_constraints.orientation_constraints[
0].header.frame_id = "base_link"
motion_plan_request.goal_constraints.orientation_constraints[
0].link_name = "right_arm_hand_link"
vals = [float() for _ in range(4)]
vals = [-.895249, -.187638, 0.092325, .393443]
mag = math.sqrt(vals[0] * vals[0] + vals[1] * vals[1] +
vals[2] * vals[2] + vals[3] * vals[3])
vals[0] /= mag
vals[1] /= mag
vals[2] /= mag
vals[3] /= mag
motion_plan_request.goal_constraints.orientation_constraints[
0].orientation.x = vals[0]
motion_plan_request.goal_constraints.orientation_constraints[
0].orientation.y = vals[1]
motion_plan_request.goal_constraints.orientation_constraints[
0].orientation.z = vals[2]
motion_plan_request.goal_constraints.orientation_constraints[
0].orientation.w = vals[3]
motion_plan_request.goal_constraints.orientation_constraints[
0].absolute_roll_tolerance = 0.04
motion_plan_request.goal_constraints.orientation_constraints[
0].absolute_pitch_tolerance = 0.04
motion_plan_request.goal_constraints.orientation_constraints[
0].absolute_yaw_tolerance = 0.04
motion_plan_request.goal_constraints.orientation_constraints[
0].weight = 1.0
goal = MoveArmGoal()
goal.planner_service_name = "ompl_planning/plan_kinematic_path"
goal.motion_plan_request = motion_plan_request
self.move_arm_action_client.send_goal(goal)
while True:
cur_state = self.move_arm_action_client.get_state()
if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE
and cur_state != actionlib_msgs.msg.GoalStatus.PENDING):
break