def init_orient_constraints(x_tol, y_tol, z_tol): if x_tol < 0.1: x_tol = 0.1 if y_tol < 0.1: y_tol = 0.1 if z_tol < 0.1: z_tol = 0.1 print x_tol, y_tol, z_tol cur_pose = manipulator.get_current_pose().pose upright_constraints = Constraints() upright_constraints.name = "upright" orientation_constraint = OrientationConstraint() # orientation_constraint.header = pose.header orientation_constraint.header.frame_id = "world" orientation_constraint.link_name = manipulator.get_end_effector_link() print "end link: ", manipulator.get_end_effector_link() orientation_constraint.orientation = cur_pose.orientation #IIWA orientation_constraint.absolute_x_axis_tolerance = x_tol orientation_constraint.absolute_y_axis_tolerance = y_tol orientation_constraint.absolute_z_axis_tolerance = z_tol orientation_constraint.weight = 1.0 upright_constraints.orientation_constraints.append(orientation_constraint) return upright_constraints
def append_pose_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None): """ Appends a pose to the given move_group goal, returns it appended Goals for now are for the same link TODO: let it be for different links""" if goal_to_append == None: rospy.logerr("append_pose_to_move_group_goal needs a goal!") return goal_to_append = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = goal_to_append.request.goal_constraints[0].header position_c.link_name = goal_to_append.request.goal_constraints[0].link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = goal_to_append.request.goal_constraints[0].header orientation_c.link_name = goal_to_append.request.goal_constraints[0].link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) goal_to_append.request.goal_constraints.append(goal_c) return goal_to_append
def append_traj_to_move_group_goal(goal_to_append=None, goal_pose=Pose(), link_name=None): """ Appends a trajectory_point to the given move_group goal, returns it appended""" if goal_to_append == None: rospy.logerr("append_trajectory_point_to_move_group_goal needs a goal!") return #goal_to_append = MoveGroupGoal() #rospy.logwarn("goal_to_append is: \n" + str(goal_to_append)) traj_c = TrajectoryConstraints() goal_c = Constraints() goal_c.name = "traj_constraint" # Position constraint position_c = PositionConstraint() position_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header position_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 2.0 goal_c.position_constraints.append(position_c) # Orientation constraint orientation_c = OrientationConstraint() orientation_c.header = goal_to_append.request.goal_constraints[0].position_constraints[0].header orientation_c.link_name = goal_to_append.request.goal_constraints[0].position_constraints[0].link_name if link_name == None else link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) traj_c.constraints.append(goal_c) goal_to_append.request.trajectory_constraints = traj_c return goal_to_append
def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True): header = Header() header.frame_id = 'torso_base_link' header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header if end_link_name != None: position_c.link_name = end_link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header if end_link_name != None: orientation_c.link_name = end_link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 5 moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary moveit_goal.request.group_name = group return moveit_goal
def init_upright_path_constraints(): cur_pose = manipulator.get_current_pose().pose upright_constraints = Constraints() upright_constraints.name = "upright" orientation_constraint = OrientationConstraint() # orientation_constraint.header = pose.header orientation_constraint.header.frame_id = "world" orientation_constraint.link_name = manipulator.get_end_effector_link() print "end link: ", manipulator.get_end_effector_link() orientation_constraint.orientation = cur_pose.orientation #IIWA orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 #UR5 # orientation_constraint.absolute_x_axis_tolerance = 3.14 # orientation_constraint.absolute_y_axis_tolerance = 0.1 # orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 1.0 upright_constraints.orientation_constraints.append(orientation_constraint) return upright_constraints
def set_orientation_goal(self, quaternion, tolerance=0.001, weight=1.0, frame=None): """ Set goal orientation of `frame`. Defaults to the end-effector `frame`. """ if frame == None: frame = self.arm_end_effector orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = self.arm_base_link orientation_constraint.link_name = frame orientation_constraint.orientation.x = quaternion[0] orientation_constraint.orientation.y = quaternion[1] orientation_constraint.orientation.z = quaternion[2] orientation_constraint.orientation.w = quaternion[3] orientation_constraint.absolute_x_axis_tolerance = tolerance orientation_constraint.absolute_y_axis_tolerance = tolerance orientation_constraint.absolute_z_axis_tolerance = tolerance orientation_constraint.weight = weight (self.kinematic_path_request.motion_plan_request.goal_constraints[-1]. orientation_constraints.append(orientation_constraint))
def on_enter(self, userdata): self._failed = False request = GetCartesianPathRequest() request.group_name = self._move_group request.avoid_collisions = not self._ignore_collisions request.max_step = 0.05 request.header = userdata.eef_pose.header request.waypoints.append(userdata.eef_pose.pose) now = rospy.Time.now() try: self._tf.waitForTransform('base_link', 'gripper_cam_link', now, rospy.Duration(1)) (p,q) = self._tf.lookupTransform('gripper_cam_link', 'base_link', now) c = OrientationConstraint() c.header.frame_id = 'base_link' c.header.stamp = now c.link_name = 'gripper_cam_link' c.orientation.x = q[0] c.orientation.y = q[1] c.orientation.z = q[2] c.orientation.w = q[3] c.weight = 1 c.absolute_x_axis_tolerance = 0.1 c.absolute_y_axis_tolerance = 0.1 c.absolute_z_axis_tolerance = 0.1 #request.path_constraints.orientation_constraints.append(c) self._result = self._srv.call(self._topic, request) except Exception as e: Logger.logwarn('Exception while calculating path:\n%s' % str(e)) self._failed = True
def set_pose_quat_target(self, pose): ############################ rospy.logwarn("set_pose_quat_target") # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" start_pose = self.arm.get_current_pose(self.end_effector_link) rospy.logwarn(self.end_effector_link) rospy.logwarn("start_pose:") rospy.logwarn(start_pose) rospy.logwarn("pose:") rospy.logwarn(pose) # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = self.end_effector_link orientation_constraint.orientation.x = start_pose.pose.orientation.x orientation_constraint.orientation.y = start_pose.pose.orientation.y orientation_constraint.orientation.z = start_pose.pose.orientation.z orientation_constraint.orientation.w = start_pose.pose.orientation.w orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) rospy.logwarn("constraints:") rospy.logwarn(constraints) # Set the path constraints on the right_arm self.arm.set_path_constraints(constraints) ############################ self.arm.set_pose_target(pose, self.end_effector_link)
def plan_motion_constrained(commander, goal_pose): #Define Pose goal = PoseStamped() goal.header.frame_id = "base" goal.pose.position.x = goal_pose[0] goal.pose.position.y = goal_pose[1] goal.pose.position.z = goal_pose[2] goal.pose.orientation.x = goal_pose[3] goal.pose.orientation.y = goal_pose[4] goal.pose.orientation.z = goal_pose[5] goal.pose.orientation.w = goal_pose[6] commander.set_pose_target(goal) commander.set_start_state_to_current_state() orien_const = OrientationConstraint() orien_const.link_name = "right_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.y = -1.0; orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 1.0; consts = Constraints() consts.orientation_constraints = [orien_const] commander.set_path_constraints(consts) #Plan a path plan = commander.plan() #Return plan return plan
def move_ik(self, xyz): move_group = self.arm_group fixed_end_effector = OrientationConstraint() fixed_end_effector.link_name = "end_effector_link" fixed_end_effector.header.frame_id = "link1" fixed_end_effector.orientation.w = 1.0 fixed_end_effector.absolute_x_axis_tolerance = 0.1 fixed_end_effector.absolute_y_axis_tolerance = 0.1 fixed_end_effector.absolute_z_axis_tolerance = 3.14 fixed_end_effector.weight = 1.0 constraint = Constraints() constraint.name = "tilt constraint" constraint.orientation_constraints = [fixed_end_effector] move_group.set_path_constraints(constraint) move_group.set_position_target(xyz) try: move_group.go(wait=True) except MoveItCommanderException: print "sorry cant move here!" # move_group.stop() self.moving = False
def __init__(self, program_file=PROGRAM_FILE): # TODO: Either implement behavior that fixes programs when markers change # or only let this callback run once self._markers_sub = rospy.Subscriber(SUB_NAME, Marker, callback=self._markers_callback) self._curr_markers = None self._tf_listener = tf.TransformListener() self._arm = fetch_api.Arm() self._gripper = fetch_api.Gripper() self._torso = fetch_api.Torso() self._controller_client = actionlib.SimpleActionClient( '/query_controller_states', QueryControllerStatesAction) self._program_file = program_file self._programs = self._read_in_programs() self._joint_reader = JointStateReader() mat = tft.identity_matrix() mat[:, 0] = np.array([0, 0, -1, 0]) mat[:, 2] = np.array([1, 0, 0, 0]) o = tft.quaternion_from_matrix(mat) self._constraint_pose = Pose(orientation=Quaternion(*o)) oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'gripper_link' oc.orientation = self._constraint_pose.orientation oc.weight = 1.0 oc.absolute_z_axis_tolerance = 1.0 oc.absolute_x_axis_tolerance = 1.0 oc.absolute_y_axis_tolerance = 1.0 self._constraint = None
def move(self, num): moveit_commander.roscpp_initialize(sys.argv) # rospy.init_node("moveit_node") robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group = moveit_commander.MoveGroupCommander('right_arm') right_arm = baxter_interface.Limb("right") right_arm_pose = right_arm.endpoint_pose() # print(right_arm_pose) initial_orient = [ right_arm_pose['orientation'].x, right_arm_pose['orientation'].y, right_arm_pose['orientation'].z, right_arm_pose['orientation'].w ] dig_class = dg.Digits() digit_shapes = dig_class.digit num_shape = digit_shapes[str(num)] i = 0 # print(num_shape, num, len(num_shape)) while i < len(num_shape): (x, y) = num_shape[i] pose_target = PoseStamped() pose_target.header.frame_id = "base" # print(pose_target.pose.position) right_arm_pose = right_arm.endpoint_pose() x = right_arm_pose['position'].x y = right_arm_pose['position'].y z = right_arm_pose['position'].z print(right_arm_pose) pose_target.pose.position.x = x + num_shape[i][0] * 0.1 pose_target.pose.position.y = y + num_shape[i][1] * 0.1 pose_target.pose.position.z = z pose_target.pose.orientation.x = initial_orient[0] pose_target.pose.orientation.y = initial_orient[1] pose_target.pose.orientation.z = initial_orient[2] pose_target.pose.orientation.w = initial_orient[3] group.set_pose_target(pose_target) group.set_start_state_to_current_state() orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "base" orien_const.orientation.x = initial_orient[0] orien_const.orientation.y = initial_orient[1] orien_const.orientation.z = initial_orient[2] orien_const.orientation.w = initial_orient[3] orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 consts = Constraints() consts.orientation_constraints = [orien_const] group.set_path_constraints(consts) plan = group.plan() group.execute(plan) i += 1
def get_ik(target): """ :param target: a PoseStamped give the desired position of the endeffector. """ pose = group.get_current_pose(group.get_end_effector_link()) constraints = Constraints() orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 2*pi current_orientation_list = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w] # get euler angle from quaternion (roll, pitch, yaw) = euler_from_quaternion(current_orientation_list) pitch = pi roll = 0 orientation_constraint.weight = 1 [orientation_constraint.orientation.x, orientation_constraint.orientation.y, orientation_constraint.orientation.z, orientation_constraint.orientation.w] = \ quaternion_from_euler(roll, pitch, yaw) constraints.orientation_constraints.append(orientation_constraint) # group.set_path_constraints(constraints) ##################################################################### rospy.wait_for_service('compute_ik') request_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) service_request = PositionIKRequest() service_request.group_name = 'robot' service_request.pose_stamped.header.frame_id = 'base_footprint' # service_request.pose_stamped = group.get_current_pose() service_request.robot_state = robot.get_current_state() service_request.ik_link_name = 'arm_link_5' # Set position service_request.pose_stamped.pose.position.x = target[0] service_request.pose_stamped.pose.position.y = target[1] service_request.pose_stamped.pose.position.z = target[2] service_request.pose_stamped.pose.orientation.w =1 service_request.constraints.orientation_constraints.append(orientation_constraint) service_request.timeout.secs= 4 service_request.attempts= 2 service_request.avoid_collisions = True resp = request_ik(service_request) return resp
def _to_ori_constraint(pose, reference_frame, link_name, orientation_tolerance=_DEFAULT_ORIENTATION_TOLERANCE): """Returns an orientation constraint suitable for ActionGoal's.""" ori_con = OrientationConstraint() ori_con.header.frame_id = reference_frame ori_con.link_name = link_name ori_con.orientation = pose.orientation ori_con.absolute_x_axis_tolerance = orientation_tolerance ori_con.absolute_y_axis_tolerance = orientation_tolerance ori_con.absolute_z_axis_tolerance = orientation_tolerance ori_con.weight = 1 return ori_con
def staticDip(self, z_pose=0.1, tolerance=0.075): group = self.group current_pose = group.get_current_pose().pose position_constraint = PositionConstraint() position_constraint.target_point_offset.x = 0.1 position_constraint.target_point_offset.y = 0.1 position_constraint.target_point_offset.z = 0.5 position_constraint.weight = 0.25 position_constraint.link_name = group.get_end_effector_link() position_constraint.header.frame_id = group.get_planning_frame() orientation_constraint = OrientationConstraint() orientation_constraint.orientation = Quaternion(x=self.q[0], y=self.q[1], z=self.q[2], w=self.q[3]) orientation_constraint.absolute_x_axis_tolerance = 0.3 orientation_constraint.absolute_y_axis_tolerance = 0.3 orientation_constraint.absolute_z_axis_tolerance = 0.3 orientation_constraint.weight = 0.5 orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.header.frame_id = group.get_planning_frame() constraint = Constraints() constraint.orientation_constraints.append(orientation_constraint) constraint.position_constraints.append(position_constraint) group.set_path_constraints(constraint) allow_replanning = False planning_time = 12 before_dip = current_pose.position.z # dip = False # while not dip: dip = self.go_to_pose_goal( current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w, self.target_location_x, # accounting for tolerance error self.target_location_y, # accounting for tolerance error z_pose, # This is where we dip allow_replanning, planning_time, tolerance / 3) # current_pose = group.get_current_pose().pose rospy.sleep(0.01) group.clear_path_constraints() after_dip = group.get_current_pose().pose.position.z if dip and (before_dip > after_dip): # print "\nBefore dip z pose: ",before_dip # print "\nAfter dip z pose: ",after_dip # print "\nSuccessfully dipped!" return True else: self.staticDip()
def move_to_coord(trans, rot, arm, which_arm='left', keep_oreint=False, base="base"): #coordinates are in baxter's torso! goal = PoseStamped() goal.header.frame_id = base # x, y, and z position goal.pose.position.x = trans[0] goal.pose.position.y = trans[1] goal.pose.position.z = trans[2] # Orientation as a quaternion goal.pose.orientation.x = rot[0] goal.pose.orientation.y = rot[1] goal.pose.orientation.z = rot[2] goal.pose.orientation.w = rot[3] # Set the goal state to the pose you just defined arm.set_pose_target(goal) # Set the start state for the arm arm.set_start_state_to_current_state() if keep_oreint: # Create a path constraint for the arm orien_const = OrientationConstraint() orien_const.link_name = which_arm + "_gripper" orien_const.header.frame_id = "base" #constrain it to be the same as my goal state. Seems reasonable. orien_const.orientation.x = rot[0] orien_const.orientation.y = rot[1] orien_const.orientation.z = rot[2] orien_const.orientation.w = rot[3] orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 consts = Constraints() consts.orientation_constraints = [orien_const] print(consts) arm.set_path_constraints(consts) # Plan a path arm_plan = arm.plan() # Execute the plan arm.execute(arm_plan)
def execute(self, userdata): # Prepare the position goal_pose = Pose() goal_pose.position = userdata.manip_goal_pose # Set the rotation of the tool link, all 0 means for the right hand palm looking left straight # roll = rotation over X, pitch = rotation over Y, yaw = rotation over Z quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw goal_pose.orientation = Quaternion(*quat.tolist()) header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() userdata.move_it_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header if userdata.manip_end_link != None: # For some groups the end_link_name can be deduced, but better add it manually position_c.link_name = userdata.manip_end_link # how big is the area where the end effector can be position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header if userdata.manip_end_link != None: orientation_c.link_name = userdata.manip_end_link orientation_c.orientation = goal_pose.orientation # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) userdata.move_it_goal.request.goal_constraints.append(goal_c) userdata.move_it_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported. userdata.move_it_goal.request.allowed_planning_time = 5.0 userdata.move_it_goal.planning_options.plan_only = False # True: Plan-Only..... False : Plan + execute userdata.move_it_goal.planning_options.planning_scene_diff.is_diff = True # Necessary userdata.move_it_goal.request.group_name = userdata.manip_group return 'succeeded'
def liftgripper(self, threshold=0.055): # approx centers of onions at 0.82, width of onion is 0.038 m. table is at 0.78 # length of gripper is 0.163 m The gripper should not go lower than # (height_z of table w.r.t base+gripper-height/2+tolerance) = 0.78-0.93+0.08+0.01=-0.24 # pnp._limb.endpoint_pose returns {'position': (x, y, z), 'orientation': (x, y, z, w)} # moving from z=-.02 to z=-0.1 # print "Attempting to lift gripper" group = self.group while self.target_location_x == -100: rospy.sleep(0.05) position_constraint = PositionConstraint() position_constraint.target_point_offset.x = 0.1 position_constraint.target_point_offset.y = 0.1 position_constraint.target_point_offset.z = 0.5 position_constraint.weight = 0.25 position_constraint.link_name = group.get_end_effector_link() position_constraint.header.frame_id = group.get_planning_frame() orientation_constraint = OrientationConstraint() orientation_constraint.orientation = Quaternion(x=self.q[0], y=self.q[1], z=self.q[2], w=self.q[3]) orientation_constraint.absolute_x_axis_tolerance = 0.3 orientation_constraint.absolute_y_axis_tolerance = 0.3 orientation_constraint.absolute_z_axis_tolerance = 0.3 orientation_constraint.weight = 0.5 # Empirically estimated values for Sawyer Robot orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.header.frame_id = group.get_planning_frame() constraint = Constraints() constraint.orientation_constraints.append(orientation_constraint) constraint.position_constraints.append(position_constraint) group.set_path_constraints(constraint) current_pose = group.get_current_pose().pose allow_replanning = False planning_time = 12 lifted = False # print "Current z pose: ", current_pose.position.z z_pose = current_pose.position.z + 0.25 while not lifted: lifted = self.go_to_pose_goal( current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w, current_pose.position.x, current_pose.position.y, z_pose, allow_replanning, planning_time, threshold) # current_pose = group.get_current_pose().pose rospy.sleep(0.01) group.clear_path_constraints() # print "Successfully lifted gripper to z: ", current_pose.position.z return True
def move_to_coord(trans, rot, keep_oreint=False): #coordinates are in baxter's torso! global arm goal = PoseStamped() goal.header.frame_id = "base" # x, y, and z position goal.pose.position.x = trans[0] goal.pose.position.y = trans[1] goal.pose.position.z = trans[2] # Orientation as a quaternion goal.pose.orientation.x = rot[0] goal.pose.orientation.y = rot[1] goal.pose.orientation.z = rot[2] goal.pose.orientation.w = rot[3] # Set the goal state to the pose you just defined arm.set_pose_target(goal) # Set the start state for the arm arm.set_start_state_to_current_state() if keep_oreint: # Create a path constraint for the arm orien_const = OrientationConstraint() orien_const.link_name = 'left'+"_gripper"; orien_const.header.frame_id = "base"; #constrain it to be the same as my goal state. Seems reasonable. orien_const.orientation.x = rot[0]; orien_const.orientation.y = rot[1]; orien_const.orientation.z = rot[2]; orien_const.orientation.w = rot[3]; orien_const.absolute_x_axis_tolerance = 0.02; orien_const.absolute_y_axis_tolerance = 0.02; orien_const.absolute_z_axis_tolerance = 0.02; orien_const.weight = 1.0; consts = Constraints() consts.orientation_constraints = [orien_const] print(consts) arm.set_path_constraints(consts) # Plan a path arm_plan = arm.plan() # Execute the plan #arm.execute(arm_plan) arm.go(arm_plan,wait=True)
def create_constraint(name): orien_const = OrientationConstraint() orien_const.link_name = name; orien_const.header.frame_id = "base"; orien_const.orientation.y = 2**.5/2 orien_const.orientation.w = 2**.5/2 orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 3.0; consts = Constraints() consts.orientation_constraints = [orien_const] return consts
def right_to_player(right_arm, right_gripper): # get current number of cards player has global player_cards # get global deck coordinates global deck_x global deck_y global deck_z # create pose above drop point for player's card card = PoseStamped() card.header.frame_id = "base" # x, y, and z position of card card.pose.position.x = deck_x + .25 card.pose.position.y = deck_y + .2 + (.1 * player_cards) card.pose.position.z = deck_z # Orientation as a quaternion - straight down card.pose.orientation.x = 0.0 card.pose.orientation.y = -1.0 card.pose.orientation.z = 0.0 card.pose.orientation.w = 0.0 # Plan and execute path to desired card position from current state right_arm.set_pose_target(card) right_arm.set_start_state_to_current_state() # Add orientation constraint for path - straight down orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 consts = Constraints() consts.orientation_constraints = [orien_const] right_arm.set_path_constraints(consts) right_plan = right_arm.plan() right_arm.execute(right_plan) # drop card right_gripper.stop() # sleep for a moment rospy.sleep(1.0) # increment number of cards player has player_cards += 1
def staticDip(self, z_pose=0.1, tolerance=0.05): group = self.group while self.target_location_x == -100: rospy.sleep(0.05) current_pose = group.get_current_pose().pose position_constraint = PositionConstraint() position_constraint.target_point_offset.x = 0.1 position_constraint.target_point_offset.y = 0.1 position_constraint.target_point_offset.z = 0.5 position_constraint.weight = 0.1 position_constraint.link_name = group.get_end_effector_link() position_constraint.header.frame_id = group.get_planning_frame() orientation_constraint = OrientationConstraint() orientation_constraint.orientation = Quaternion(x=self.q[0], y=self.q[1], z=self.q[2], w=self.q[3]) orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 0.3 orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.header.frame_id = group.get_planning_frame() constraint = Constraints() constraint.orientation_constraints.append(orientation_constraint) constraint.position_constraints.append(position_constraint) group.set_path_constraints(constraint) allow_replanning = False planning_time = 10 # print "Now performing dip" before_dip = current_pose.position.z dip = self.go_to_pose_goal( self.q[0], self.q[1], self.q[2], self.q[3], self.target_location_x + 0.01, # accounting for tolerance error self.target_location_y, # accounting for tolerance error z_pose, # This is where we dip allow_replanning, planning_time, tolerance) rospy.sleep(0.5) dipped_pose = group.get_current_pose().pose.position.z if dip and (dipped_pose < before_dip): print "Successfully dipped! z pos: ", dipped_pose group.clear_path_constraints() return True else: self.staticDip()
def set_upright_constraints(self,pose): upright_constraints = Constraints() orientation_constraint = OrientationConstraint() upright_constraints.name = "upright" orientation_constraint.header = pose.header orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 3.14 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 #orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis orientation_constraint.weight = 1 upright_constraints.orientation_constraints.append(orientation_constraint) group.set_path_constraints(upright_constraints)
def set_orientation_constraints(self): oc = OrientationConstraint() oc.header.frame_id = 'world' oc.link_name = 'ee_arm_link' quat = tf.transformations.quaternion_from_euler(math.radians(0), math.radians(-28), math.radians(0)) oc.orientation.x = quat[0] oc.orientation.y = quat[1] oc.orientation.z = quat[2] oc.orientation.w = quat[3] oc.absolute_x_axis_tolerance = 0.5 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 0.5 oc.weight = 1.0 self.constraints.orientation_constraints.append(oc)
def planMoveToPosHoldOrientation(pos=None, orientation=None, arm=None): arm = arm or left_arm if arm is left_arm: pos = pos or left_arm_default_state[0] orientation = orientation or left_arm_default_state[1] if arm is right_arm: pos = pos or right_arm_default_state[0] orientation = orientation or right_arm_default_state[1] #Second goal pose ----------------------------------------------------- goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = pos[0] goal_2.pose.position.y = pos[1] goal_2.pose.position.z = pos[2] #Orientation as a quaternion orientation = orientation/np.linalg.norm(orientation) goal_2.pose.orientation.x = orientation[0] goal_2.pose.orientation.y = orientation[1] goal_2.pose.orientation.z = orientation[2] goal_2.pose.orientation.w = orientation[3] #Set the goal state to the pose you just defined arm.set_pose_target(goal_2) #Set the start state for the left arm arm.set_start_state_to_current_state() # #Create a path constraint for the arm # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS orien_const = OrientationConstraint() orien_const.link_name = "left_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.x = orientation[0] orien_const.orientation.y = orientation[1] orien_const.orientation.z = orientation[2] orien_const.orientation.w = orientation[3] orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 1.0; consts = Constraints() consts.orientation_constraints = [orien_const] arm.set_path_constraints(consts) #Plan a path return (arm, arm.plan())
def right_to_deck(right_arm, right_gripper): # get global deck coordinates global deck_x global deck_y global deck_z # create pose above deck of cards deck = PoseStamped() deck.header.frame_id = "base" # x, y, and z position of deck deck.pose.position.x = deck_x deck.pose.position.y = deck_y deck.pose.position.z = deck_z # Orientation as a quaternion - straight down deck.pose.orientation.x = 0.0 deck.pose.orientation.y = -1.0 deck.pose.orientation.z = 0.0 deck.pose.orientation.w = 0.0 # Plan and execute path from current state right_arm.set_pose_target(deck) right_arm.set_start_state_to_current_state() # Add orientation constraint for path - straight down orien_const = OrientationConstraint() orien_const.link_name = "right_gripper" orien_const.header.frame_id = "base" orien_const.orientation.y = -1.0 orien_const.absolute_x_axis_tolerance = 0.1 orien_const.absolute_y_axis_tolerance = 0.1 orien_const.absolute_z_axis_tolerance = 0.1 orien_const.weight = 1.0 consts = Constraints() consts.orientation_constraints = [orien_const] right_arm.set_path_constraints(consts) right_plan = right_arm.plan() right_arm.execute(right_plan) # pick up card right_gripper.command_suction(True) # increment z position as deck height decreases deck_z -= .001 # sleep for a moment rospy.sleep(1.0)
def init_path_constraints(self): self.upright_constraints.name = 'upright' orientation_constraint = OrientationConstraint() current_pose = self.group.get_current_pose().pose planning_frame = self.group.get_planning_frame() orientation_constraint.header.frame_id = planning_frame orientation_constraint.link_name = self.group.get_end_effector_link() print 'Constraint initialized: ' + str(orientation_constraint.link_name) orientation_constraint.orientation = current_pose.orientation orientation_constraint.absolute_x_axis_tolerance = PATH_CONSTRAINT_TOLERANCE orientation_constraint.absolute_y_axis_tolerance = PATH_CONSTRAINT_TOLERANCE orientation_constraint.absolute_z_axis_tolerance = PATH_CONSTRAINT_TOLERANCE orientation_constraint.weight = 0.01 self.upright_constraints.orientation_constraints.append(orientation_constraint) self.group.set_path_constraints(self.upright_constraints)
def createOrientationConstraint(): orientation_constraints = [] o = OrientationConstraint() o.header.frame_id = 'base_link' o.link_name = 'arm_right_tool_link' o.orientation.x = 0.0 o.orientation.y = 0.0 o.orientation.z = 0.0 o.orientation.w = 1.0 o.weight = 1.0 o.absolute_x_axis_tolerance = 0.1 o.absolute_y_axis_tolerance = 0.1 o.absolute_z_axis_tolerance = 0.1 orientation_constraints.append(o) return orientation_constraints
def set_path_ori_constraints(self, stampedPoseList): self.__graspPathConstraints.orientation_constraints = [] ori_constraints = [] for stampedPose in stampedPoseList: constraint = OrientationConstraint() constraint.header = stampedPose.header constraint.orientation = deepcopy(stampedPose.pose.orientation) constraint.link_name = "world" constraint.absolute_x_axis_tolerance = 0.2 constraint.absolute_y_axis_tolerance = 0.2 constraint.absolute_z_axis_tolerance = 0.2 constraint.weight = 0.8 ori_constraints.append(constraint) # ipdb.set_trace() self.__graspPathConstraints.orientation_constraints = ori_constraints
def planPath(positions, orientation=None, arm=None, holdOrientation=False, step=0.01, threshold=1000): arm = arm or left_arm if arm is left_arm: positions = (left_arm_default_state[0],) if positions is None else positions orientation = left_arm_default_state[1] if orientation is None else orientation if arm is right_arm: positions = (right_arm_default_state[0],) if positions is None else positions orientation = right_arm_default_state[1] if orientation is None else orientation # Compute path waypoints = [] # start with the current pose #waypoints.append(arm.get_current_pose().pose) wpose = Pose() orientation = orientation/np.linalg.norm(orientation) wpose.orientation.x = orientation[0] wpose.orientation.y = orientation[1] wpose.orientation.z = orientation[2] wpose.orientation.w = orientation[3] for pos in positions: wpose.position.x = pos[0] wpose.position.y = pos[1] wpose.position.z = pos[2] waypoints.append(copy.deepcopy(wpose)) if holdOrientation: # #Create a path constraint for the arm # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS orien_const = OrientationConstraint() orien_const.link_name = "left_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.x = orientation[0] orien_const.orientation.y = orientation[1] orien_const.orientation.z = orientation[2] orien_const.orientation.w = orientation[3] orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 1.0; consts = Constraints() consts.orientation_constraints = [orien_const] arm.set_path_constraints(consts) #Plan a path (plan, fraction) = arm.compute_cartesian_path(waypoints, step, threshold) return (arm, plan, fraction)
def place(robot): p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.7 p.pose.position.y = 0.0 p.pose.position.z = 0.5 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1.0 g = PlaceLocation() g.place_pose = p g.pre_place_approach.direction.vector.z = -1.0 g.post_place_retreat.direction.vector.x = -1.0 g.post_place_retreat.direction.header.frame_id = robot.get_planning_frame() g.pre_place_approach.direction.header.frame_id = "r_wrist_roll_link" g.pre_place_approach.min_distance = 0.1 g.pre_place_approach.desired_distance = 0.2 g.post_place_retreat.min_distance = 0.1 g.post_place_retreat.desired_distance = 0.25 g.post_place_posture = open_gripper(g.post_place_posture) group = robot.get_group("right_arm") group.set_support_surface_name("table") # Add path constraints constr = Constraints() constr.orientation_constraints = [] ocm = OrientationConstraint() ocm.link_name = "r_wrist_roll_link" ocm.header.frame_id = p.header.frame_id ocm.orientation.x = 0.0 ocm.orientation.y = 0.0 ocm.orientation.z = 0.0 ocm.orientation.w = 1.0 ocm.absolute_x_axis_tolerance = 0.2 ocm.absolute_y_axis_tolerance = 0.2 ocm.absolute_z_axis_tolerance = math.pi ocm.weight = 1.0 constr.orientation_constraints.append(ocm) # group.set_path_constraints(constr) group.set_planner_id("RRTConnectkConfigDefault") group.place("part", g)
def add_robot_constraints(): constraint = Constraints() constraint.name = "camera constraint" roll_constraint = OrientationConstraint() # 'base_link' is equal to the world link roll_constraint.header.frame_id = 'world' # The link that must be oriented upwards roll_constraint.link_name = "camera" roll_constraint.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0,np.pi/3,0)) # Allow rotation of 45 degrees around the x and y axis roll_constraint.absolute_x_axis_tolerance = np.pi #Allow max rotation of x degrees roll_constraint.absolute_y_axis_tolerance = np.pi roll_constraint.absolute_z_axis_tolerance = np.pi/2 # The roll constraint is the only constraint roll_constraint.weight = 1 #constraint.orientation_constraints = [roll_constraint] joint_1_constraint = JointConstraint() joint_1_constraint.joint_name = "joint_1" joint_1_constraint.position = 0 joint_1_constraint.tolerance_above = np.pi/2 joint_1_constraint.tolerance_below = np.pi/2 joint_1_constraint.weight = 1 joint_4_constraint = JointConstraint() joint_4_constraint.joint_name = "joint_4" joint_4_constraint.position = 0 joint_4_constraint.tolerance_above = np.pi/2 joint_4_constraint.tolerance_below = np.pi/2 joint_4_constraint.weight = 1 joint_5_constraint = JointConstraint() joint_5_constraint.joint_name = "joint_5" joint_5_constraint.position = np.pi/2 joint_5_constraint.tolerance_above = np.pi/2 joint_5_constraint.tolerance_below = np.pi/2 joint_5_constraint.weight = 1 joint_6_constraint = JointConstraint() joint_6_constraint.joint_name = "joint_6" joint_6_constraint.position = np.pi-0.79 joint_6_constraint.tolerance_above = np.pi joint_6_constraint.tolerance_below = np.pi joint_6_constraint.weight = 1 constraint.joint_constraints = [joint_1_constraint, joint_6_constraint] return constraint
def create_move_group_pose_goal(goal_pose=Pose(), group="right_arm_torso", end_link_name=None, plan_only=True): """ Creates a move_group goal based on pose. @arg group string representing the move_group group to use @arg end_link_name string representing the ending link to use @arg goal_pose Pose() representing the goal pose @arg plan_only bool to for only planning or planning and executing @returns MoveGroupGoal with the data given on the arguments""" header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() # We are filling in the MoveGroupGoal a MotionPlanRequest and a PlanningOptions message # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/MotionPlanRequest.html # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/PlanningOptions.html moveit_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header if end_link_name != None: # For some groups the end_link_name can be deduced, but better add it manually position_c.link_name = end_link_name position_c.constraint_region.primitives.append( SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[ 0.01 ])) # how big is the area where the end effector can be position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header if end_link_name != None: orientation_c.link_name = end_link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported. moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary moveit_goal.request.group_name = group return moveit_goal
def addConstrains(): pose = group.get_current_pose() constraint = Constraints() constraint.name = "downRight" orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 #orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis orientation_constraint.weight = 1 constraint.orientation_constraints.append(orientation_constraint) group.set_path_constraints(constraint)
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_test') arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() arm.allow_replanning(True) arm.set_planning_time(5) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_init_orientation = Quaternion() target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0) self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation)) current_pose = arm.get_current_pose(end_effector_link) self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation)) arm.set_pose_target(current_pose) arm.go() rospy.sleep(2) constraints = Constraints() orientation_constraint = OrientationConstraint() constraints.name = 'gripper constraint' orientation_constraint.header = target_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.orientation.x = target_init_orientation[0] orientation_constraint.orientation.y = target_init_orientation[1] orientation_constraint.orientation.z = target_init_orientation[2] orientation_constraint.orientation.w = target_init_orientation[3] orientation_constraint.absolute_x_axis_tolerance = 0.03 orientation_constraint.absolute_y_axis_tolerance = 0.03 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) arm.set_path_constraints(constraints) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def setOrientationConstraints(qw,qx,qy,qz, group_handle,weight): orient_constrain = OrientationConstraint(); orient_constrain.header.frame_id = "base_link"; orient_constrain.link_name = group_handle.get_end_effector_link(); orient_constrain.orientation = Quaternion(); orient_constrain.orientation.w = qw; orient_constrain.orientation.x = qx; orient_constrain.orientation.y = qy; orient_constrain.orientation.z = qz; orient_constrain.absolute_x_axis_tolerance = 0.01; orient_constrain.absolute_y_axis_tolerance = 0.01; orient_constrain.absolute_z_axis_tolerance = 2*pi; orient_constrain.weight = weight; return orient_constrain;
def freeze_orientation(self): # get current pose cur_pose = self._group.get_current_pose() # create pose constraint constraints = Constraints() constraints.name = "orientation_constraints" orientation_constraint = OrientationConstraint() orientation_constraint.header = cur_pose.header orientation_constraint.header.stamp = rospy.Time.now() orientation_constraint.link_name = self._group.get_end_effector_link() orientation_constraint.orientation = cur_pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 0.5 orientation_constraint.absolute_y_axis_tolerance = 0.5 orientation_constraint.absolute_z_axis_tolerance = 0.5 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) self._group.set_path_constraints(constraints) self.orientation_frozen = True
def create_move_group_pose_goal(goal_pose=Pose(), group="right_arm_torso", end_link_name=None, plan_only=True): """ Creates a move_group goal based on pose. @arg group string representing the move_group group to use @arg end_link_name string representing the ending link to use @arg goal_pose Pose() representing the goal pose @arg plan_only bool to for only planning or planning and executing @returns MoveGroupGoal with the data given on the arguments""" header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() # We are filling in the MoveGroupGoal a MotionPlanRequest and a PlanningOptions message # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/MotionPlanRequest.html # http://docs.ros.org/hydro/api/moveit_msgs/html/msg/PlanningOptions.html moveit_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header if end_link_name != None: # For some groups the end_link_name can be deduced, but better add it manually position_c.link_name = end_link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) # how big is the area where the end effector can be position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header if end_link_name != None: orientation_c.link_name = end_link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 # Tolerances, MoveIt! by default uses 0.001 which may be too low sometimes orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 5 # The number of times this plan is to be computed. Shortest solution will be reported. moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary moveit_goal.request.group_name = group return moveit_goal
def move_to_coord(trans, rot, arm, which_arm='left', keep_oreint=False): goal = PoseStamped() goal.header.frame_id = "base" # x, y, and z position goal.pose.position.x = trans[0] goal.pose.position.y = trans[1] goal.pose.position.z = trans[2] # Orientation as a quaternion goal.pose.orientation.x = rot[0] goal.pose.orientation.y = rot[1] goal.pose.orientation.z = rot[2] goal.pose.orientation.w = rot[3] # Set the goal state to the pose you just defined arm.set_pose_target(goal) # Set the start state for the arm arm.set_start_state_to_current_state() if keep_oreint: # Create a path constraint for the arm orien_const = OrientationConstraint() orien_const.link_name = which_arm+"_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.y = -1.0; orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 1.0; consts = Constraints() consts.orientation_constraints = [orien_const] arm.set_path_constraints(consts) # Plan a path arm_plan = arm.plan() # Execute the plan arm.execute(arm_plan)
def create_move_group_pose_goal( self, goal_pose=Pose(), group="left_arm", end_link_name="arm_left_tool_link", plan_only=True ): """ Creates a move_group goal based on pose. @arg group string representing the move_group group to use @arg end_link_name string representing the ending link to use @arg goal_pose Pose() representing the goal pose""" # Specifying the header makes the planning fail... header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header position_c.link_name = end_link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header orientation_c.link_name = end_link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 3 moveit_goal.request.allowed_planning_time = 1.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True moveit_goal.request.group_name = group return moveit_goal
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_constraints_demo', anonymous=True) robot = RobotCommander() # Connect to the arm move group arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since constraint planning can take a while arm.set_planning_time(5) # Allow replanning to increase the odds of a solution arm.allow_replanning(True) # Set the right arm reference frame arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) arm.set_goal_position_tolerance(0.05) arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.263803774718 target_pose.pose.position.y = 0.295405791959 target_pose.pose.position.z = 0.690438884208 q = quaternion_from_euler(0, 0, -1.57079633) target_pose.pose.orientation.x = q[0] target_pose.pose.orientation.y = q[1] target_pose.pose.orientation.z = q[2] target_pose.pose.orientation.w = q[3] # Set the start state and target pose, then plan and execute arm.set_start_state(robot.get_current_state()) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(2) # Close the gripper gripper.set_joint_value_target(GRIPPER_CLOSED) gripper.go() rospy.sleep(1) # Store the current pose start_pose = arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 0.1 orientation_constraint.weight = 1.0 # q = quaternion_from_euler(0, 0, -1.57079633) # orientation_constraint.orientation.x = q[0] # orientation_constraint.orientation.y = q[1] # orientation_constraint.orientation.z = q[2] # orientation_constraint.orientation.w = q[3] # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the arm arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.39000848183 target_pose.pose.position.y = 0.185900663329 target_pose.pose.position.z = 0.732752341378 target_pose.pose.orientation.w = 1 # Set the start state and target pose, then plan and execute arm.set_start_state_to_current_state() arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) # Clear all path constraints arm.clear_path_constraints() # Open the gripper gripper.set_joint_value_target(GRIPPER_NEUTRAL) gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file arm.set_named_target('l_arm_init') # Plan and execute a trajectory to the goal configuration arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def main(): #Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) #Start a node rospy.init_node('moveit_node') #Set up the left gripper left_gripper = baxter_gripper.Gripper('left') #Calibrate the gripper (other commands won't work unless you do this first) print('Calibrating...') left_gripper.calibrate() rospy.sleep(2.0) #Initialize both arms robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() left_arm = moveit_commander.MoveGroupCommander('left_arm') right_arm = moveit_commander.MoveGroupCommander('right_arm') left_arm.set_planner_id('RRTConnectkConfigDefault') left_arm.set_planning_time(10) right_arm.set_planner_id('RRTConnectkConfigDefault') right_arm.set_planning_time(10) #First goal pose ------------------------------------------------------ goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = 0.2 goal_1.pose.position.y = 0.6 goal_1.pose.position.z = 0.2 #Orientation as a quaternion goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -1.0 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.0 #Set the goal state to the pose you just defined left_arm.set_pose_target(goal_1) #Set the start state for the left arm left_arm.set_start_state_to_current_state() #Plan a path left_plan = left_arm.plan() #Execute the plan raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ') left_arm.execute(left_plan) #Second goal pose ----------------------------------------------------- rospy.sleep(2.0) #Close the left gripper #print('Closing...') #left_gripper.close(block=True) #rospy.sleep(0.5) #Open the left gripper #print('Opening...') #left_gripper.open(block=True) #rospy.sleep(1.0) print('Done!') goal_2 = PoseStamped() goal_2.header.frame_id = "base" #x, y, and z position goal_2.pose.position.x = 0.6 goal_2.pose.position.y = 0.4 goal_2.pose.position.z = 0.0 #Orientation as a quaternion goal_2.pose.orientation.x = 0.0 goal_2.pose.orientation.y = -1.0 goal_2.pose.orientation.z = 0.0 goal_2.pose.orientation.w = 0.0 #Set the goal state to the pose you just defined left_arm.set_pose_target(goal_2) #Set the start state for the left arm left_arm.set_start_state_to_current_state() # #Create a path constraint for the arm # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS orien_const = OrientationConstraint() orien_const.link_name = "left_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.y = -1.0; orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 1.0; consts = Constraints() consts.orientation_constraints = [orien_const] left_arm.set_path_constraints(consts) #Plan a path left_plan = left_arm.plan() #Execute the plan raw_input('Press <Enter> to move the left arm to goal pose 2: ') left_arm.execute(left_plan) #Third goal pose ----------------------------------------------------- rospy.sleep(2.0) #Close the left gripper #print('Closing...') #left_gripper.close(block=True) #rospy.sleep(0.5) #Open the left gripper #print('Opening...') #left_gripper.open(block=True) #rospy.sleep(1.0) #print('Done!') goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position goal_3.pose.position.x = 0.0 goal_3.pose.position.y = 0.7 goal_3.pose.position.z = 0.0 #Orientation as a quaternion goal_3.pose.orientation.x = 0.0 goal_3.pose.orientation.y = -1.0 goal_3.pose.orientation.z = 0.0 goal_3.pose.orientation.w = 0.0 #Set the goal state to the pose you just defined left_arm.set_pose_target(goal_3) #Set the start state for the left arm left_arm.set_start_state_to_current_state() #Create a path constraint for the arm # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS orien_const = OrientationConstraint() orien_const.link_name = "left_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.y = -1.0; orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 1.0; consts = Constraints() consts.orientation_constraints = [orien_const] left_arm.set_path_constraints(consts) #Plan a path left_plan = left_arm.plan() #Execute the plan raw_input('Press <Enter> to move the left arm to goal pose 3: ') left_arm.execute(left_plan) #Fourth goal pose ----------------------------------------------------- rospy.sleep(2.0) #Close the left gripper #print('Closing...') #left_gripper.close(block=True) #rospy.sleep(0.5) #Open the left gripper #print('Opening...') #left_gripper.open(block=True) #rospy.sleep(1.0) #print('Done!') goal_4 = PoseStamped() goal_4.header.frame_id = "base" #x, y, and z position goal_4.pose.position.x = 0.4 goal_4.pose.position.y = 0.7 goal_4.pose.position.z = 0.3 #Orientation as a quaternion goal_4.pose.orientation.x = 0.0 goal_4.pose.orientation.y = -1.0 goal_4.pose.orientation.z = 0.0 goal_4.pose.orientation.w = 0.0 #Set the goal state to the pose you just defined left_arm.set_pose_target(goal_4) #Set the start state for the left arm left_arm.set_start_state_to_current_state() #Create a path constraint for the arm # #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS #orien_const = OrientationConstraint() #orien_const.link_name = "left_gripper"; #orien_const.header.frame_id = "base"; #orien_const.orientation.y = -1.0; #orien_const.absolute_x_axis_tolerance = 0.1; #orien_const.absolute_y_axis_tolerance = 0.1; #orien_const.absolute_z_axis_tolerance = 0.1; #orien_const.weight = 1.0; #consts = Constraints() #consts.orientation_constraints = [orien_const] #left_arm.set_path_constraints(consts) #Plan a path left_plan = left_arm.plan() #Execute the plan raw_input('Press <Enter> to move the left arm to goal pose 4: ') left_arm.execute(left_plan) rospy.sleep(2.0) #Close the left gripper print('Closing...') left_gripper.close(block=True) rospy.sleep(0.5) #Open the left gripper print('Opening...') left_gripper.open(block=True) rospy.sleep(1.0) print('Done!')
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize the ROS node rospy.init_node('moveit_demo', anonymous=True) robot = RobotCommander() # Connect to the right_arm move group right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Increase the planning time since contraint planning can take a while right_arm.set_planning_time(15) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow some leeway in position(meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Start in the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Set an initial target pose with the arm up and to the right target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.237012590198 target_pose.pose.position.y = -0.0747191267505 target_pose.pose.position.z = 0.901578401949 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state(robot.get_current_state()) right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # Close the gripper right_gripper.set_joint_value_target(GRIPPER_CLOSED) right_gripper.go() rospy.sleep(1) # Store the current pose start_pose = right_arm.get_current_pose(end_effector_link) # Create a contraints list and give it a name constraints = Constraints() constraints.name = "Keep gripper horizontal" # Create an orientation constraint for the right gripper orientation_constraint = OrientationConstraint() orientation_constraint.header = start_pose.header orientation_constraint.link_name = right_arm.get_end_effector_link() orientation_constraint.orientation.w = 1.0 orientation_constraint.absolute_x_axis_tolerance = 0.1 orientation_constraint.absolute_y_axis_tolerance = 0.1 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 # Append the constraint to the list of contraints constraints.orientation_constraints.append(orientation_constraint) # Set the path constraints on the right_arm right_arm.set_path_constraints(constraints) # Set a target pose for the arm target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.173187824708 target_pose.pose.position.y = -0.0159929871606 target_pose.pose.position.z = 0.692596608605 target_pose.pose.orientation.w = 1.0 # Set the start state and target pose, then plan and execute right_arm.set_start_state_to_current_state() right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(1) # Clear all path constraints right_arm.clear_path_constraints() # Open the gripper right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('resting') # Plan and execute a trajectory to the goal configuration right_arm.go() rospy.sleep(1) # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit MoveIt moveit_commander.os._exit(0)
def callback(message): global last_x global last_y global last_z global error try: if message.transforms[0].child_frame_id == 'ar_marker_23': #Read the position of artag (trans,rot) = listener.lookupTransform('/base', '/ar_marker_23', rospy.Time(0)) #Execute only when the difference of the current position and the last position exceed the threshold if ((abs(trans[0]-last_x) < threshold) and (abs(trans[1]-last_y) < threshold) and (abs(trans[2]-last_z) < threshold)): pass else: last_x = trans[0] last_y = trans[1] last_z = trans[2] print trans #Goal position goal = PoseStamped() goal.header.frame_id = "base" #x, y, and z position goal.pose.position.x = trans[0] goal.pose.position.y = trans[1] goal.pose.position.z = trans[2]-0.1 #Orientation as a quaternion: default orientation goal.pose.orientation.x = 0.5 goal.pose.orientation.y = 0.5 goal.pose.orientation.z = 0.5 goal.pose.orientation.w = -0.5 #Set the goal state right_arm.set_pose_target(goal) #Set the start state right_arm.set_start_state_to_current_state() #Create a orientation constraint for the arm orien_const = OrientationConstraint() orien_const.link_name = "right_gripper"; orien_const.header.frame_id = "base"; orien_const.orientation.x = 0.5; orien_const.orientation.y = 0.5; orien_const.orientation.z = 0.5; orien_const.orientation.w = -0.5; orien_const.absolute_x_axis_tolerance = 0.1; orien_const.absolute_y_axis_tolerance = 0.1; orien_const.absolute_z_axis_tolerance = 0.1; orien_const.weight = 1.0; consts = Constraints() consts.orientation_constraints = [orien_const] right_arm.set_path_constraints(consts) #Plan a path right_plan = right_arm.plan() #Execute the plan right_arm.execute(right_plan) else: pass except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print 'exception'