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 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 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 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 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_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 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 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 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 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 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 __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 motionPlanToPose(self, pose, tolerance=0.01, wait=True, **kwargs): """ Move the arm to set of joint position goals :param joints: joint names for which the target position is specified. :param positions: target joint positions :param tolerance: allowed tolerance in the final joint positions. :param wait: if enabled, makes the fuctions wait until the target joint position is reached :type joints: list of string element type :type positions: list of float element type :type tolerance: float :type wait: bool """ # Check arguments supported_args = ( "max_velocity_scaling_factor", "max_acceleration_scaling_factor", "planner_id", "planning_scene_diff", "planning_time", "plan_only", "start_state", ) for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("motionPlanToPose: unsupported argument: %s", arg) # Create goal g = MotionPlanRequest() # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.start_state = kwargs["start_state"] except KeyError: g.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self._fixed_frame c1.position_constraints[0].link_name = self._gripper_frame # c1.position_constraints[0].target_point_offset b = BoundingVolume() s = SolidPrimitive() s.dimensions = [tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self._fixed_frame c1.orientation_constraints[0].orientation = pose.orientation c1.orientation_constraints[0].link_name = self._gripper_frame c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance c1.orientation_constraints[0].weight = 1.0 g.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.planner_id = self.planner_id # 7. fill in request group name g.group_name = self._group # 8. fill in request number of planning attempts try: g.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.allowed_planning_time = kwargs["planning_time"] except KeyError: g.allowed_planning_time = self.planning_time # 10. Fill in velocity scaling factor try: g.max_velocity_scaling_factor = kwargs["max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 11. Fill in acceleration scaling factor try: g.max_velocity_scaling_factor = kwargs["max_acceleration_scaling_factor"] except KeyError: pass # do not fill in at all result = self._mp_service(g) traj = result.motion_plan_response.trajectory.joint_trajectory.points if len(traj) < 1: rospy.logwarn("No motion plan found.") return None return traj
def moveToPose(self, pose, tolerance=0.01, wait=True, **kwargs): """ Move the arm, based on a goal pose_stamped for the end effector. :param pose: target pose to which we want to move specified link to :param tolerance: allowed tolerance in the final joint positions. :param wait: if enabled, makes the fuctions wait until the target joint position is reached :type pose_stamped: ros message object of type PoseStamped :type gripper_frame: string :type tolerance: float :type wait: bool """ # Check arguments supported_args = ( "max_velocity_scaling_factor", "planner_id", "planning_time", "plan_only", "start_state", ) for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("moveToPose: unsupported argument: %s", arg) # Create goal g = MoveGroupGoal() # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.request.start_state = kwargs["start_state"] except KeyError: g.request.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self._fixed_frame c1.position_constraints[0].link_name = self._gripper_frame # c1.position_constraints[0].target_point_offset b = BoundingVolume() s = SolidPrimitive() s.dimensions = [tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self._fixed_frame c1.orientation_constraints[0].orientation = pose.orientation c1.orientation_constraints[0].link_name = self._gripper_frame c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance c1.orientation_constraints[0].weight = 1.0 g.request.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.request.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.request.planner_id = self.planner_id # 7. fill in request group name g.request.group_name = self._group # 8. fill in request number of planning attempts try: g.request.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.request.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.request.allowed_planning_time = kwargs["planning_time"] except KeyError: g.request.allowed_planning_time = self.planning_time # Fill in velocity scaling factor try: g.request.max_velocity_scaling_factor = kwargs[ "max_velocity_scaling_factor" ] except KeyError: pass # do not fill in at all # 10. fill in planning options diff g.planning_options.planning_scene_diff.is_diff = True g.planning_options.planning_scene_diff.robot_state.is_diff = True # 11. fill in planning options plan only try: g.planning_options.plan_only = kwargs["plan_only"] except KeyError: g.planning_options.plan_only = self.plan_only # 12. fill in other planning options g.planning_options.look_around = False g.planning_options.replan = False # 13. send goal self._action.send_goal(g) if wait: self._action.wait_for_result() result = self._action.get_result() return processResult(result) else: rospy.loginfo("Failed while waiting for action result.") return False
import traceback from moveit_msgs.msg import OrientationConstraint from geometry_msgs.msg import PoseStamped from path_planner import PathPlanner # from planning import path_planner # from path_planner import PathPlanner from baxter_interface import gripper as robot_gripper import baxter_dataflow from threading import Thread NEUTRAL_X_LEFT, NEUTRAL_Y_LEFT, NEUTRAL_Z_LEFT = 0.85, 0.3001, -0.04 NEUTRAL_X_RIGHT, NEUTRAL_Y_RIGHT, NEUTRAL_Z_RIGHT = 0.85, -0.2995, -0.044 BOARD_LEN_Y = 0.50 MOVE_RATE = 0.98 orien_const_left_vert = OrientationConstraint() orien_const_left_vert.link_name = "left_gripper"; orien_const_left_vert.header.frame_id = "base"; orien_const_left_vert.orientation.y = -1.0; orien_const_left_vert.absolute_x_axis_tolerance = 0.1; orien_const_left_vert.absolute_y_axis_tolerance = 0.1; orien_const_left_vert.absolute_z_axis_tolerance = 0.1; orien_const_left_vert.weight = 1.0; orien_const_right_vert = OrientationConstraint() orien_const_right_vert.link_name = "right_gripper"; orien_const_right_vert.header.frame_id = "base"; orien_const_right_vert.orientation.y = -1.0; orien_const_right_vert.absolute_x_axis_tolerance = 0.1; orien_const_right_vert.absolute_y_axis_tolerance = 0.1; orien_const_right_vert.absolute_z_axis_tolerance = 0.1;
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'
def both_arms_go_to_pose_goal(self): # 设置动作对象变量,此处为both_arms both_arms = self.both_arms # 获取当前各轴转角 axis_angle = both_arms.get_current_joint_values() # print axis_angle # 获取当前末端执行器位置姿态 right_pose_goal = both_arms.get_current_pose(end_effector_link="gripper_r_finger_r") left_pose_goal = both_arms.get_current_pose(end_effector_link="gripper_l_finger_r") print right_pose_goal # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55 : right_joint_const.position = 0.024 else: right_joint_const.position = 0 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32 : left_joint_const.position = 0.024 else: left_joint_const.position = 0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = right_pose_goal.pose.orientation right_orientation_const.link_name = "gripper_r_joint_r" right_orientation_const.absolute_x_axis_tolerance = 0.6 right_orientation_const.absolute_y_axis_tolerance = 0.6 right_orientation_const.absolute_z_axis_tolerance = 0.6 right_orientation_const.weight = 1 left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = left_pose_goal.pose.orientation left_orientation_const.link_name = "gripper_l_joint_r" left_orientation_const.absolute_x_axis_tolerance = 0.6 left_orientation_const.absolute_y_axis_tolerance = 0.6 left_orientation_const.absolute_z_axis_tolerance = 0.6 left_orientation_const.weight = 1 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const, left_joint_const] # consts.orientation_constraints = [right_orientation_const, left_orientation_const] both_arms.set_path_constraints(consts) # # 设置动作对象目标位置姿态 # # 右臂 # right_pose_goal.pose.orientation.x = Right_Qux # right_pose_goal.pose.orientation.y = Right_Quy # right_pose_goal.pose.orientation.z = Right_Quz # right_pose_goal.pose.orientation.w = Right_Quw # right_pose_goal.pose.position.x = Neurondata[5] # right_pose_goal.pose.position.y = Neurondata[3] # right_pose_goal.pose.position.z = Neurondata[4] # # 左臂 # left_pose_goal.pose.orientation.x = Left_Qux # left_pose_goal.pose.orientation.y = Left_Quy # left_pose_goal.pose.orientation.z = Left_Quz # left_pose_goal.pose.orientation.w = Left_Quw # left_pose_goal.pose.position.x = Neurondata[11] # left_pose_goal.pose.position.y = Neurondata[9] # left_pose_goal.pose.position.z = Neurondata[10] # # 右臂 # right_pose_goal.pose.orientation.x = Right_Qux # right_pose_goal.pose.orientation.y = Right_Quy # right_pose_goal.pose.orientation.z = Right_Quz # right_pose_goal.pose.orientation.w = Right_Quw # right_pose_goal.pose.position.x = (1266/740)*(Neurondata[5]+0.28)-0.495 # right_pose_goal.pose.position.y = (1295/780)*(Neurondata[3]+0.56)-0.754 # right_pose_goal.pose.position.z = (1355/776)*(Neurondata[4]-0.054)-0.184 # # 左臂 # left_pose_goal.pose.orientation.x = Left_Qux # left_pose_goal.pose.orientation.y = Left_Quy # left_pose_goal.pose.orientation.z = Left_Quz # left_pose_goal.pose.orientation.w = Left_Quw # left_pose_goal.pose.position.x = (1266/850)*(Neurondata[11]+0.33)-0.495 # left_pose_goal.pose.position.y = (1295/720)*(Neurondata[9]+0.19)-0.541 # left_pose_goal.pose.position.z = (1355/745)*(Neurondata[10]-0.055)-0.184 # 右臂 right_pose_goal.pose.orientation.x = Right_Qux right_pose_goal.pose.orientation.y = Right_Quy right_pose_goal.pose.orientation.z = Right_Quz right_pose_goal.pose.orientation.w = Right_Quw right_pose_goal.pose.position.x = (Neurondata[5]-0.05)*1.48+0.053 right_pose_goal.pose.position.y = (Neurondata[3]+0.18)*1.48-0.12 right_pose_goal.pose.position.z = (Neurondata[4]-0.53)*1.48+0.47 # 左臂 left_pose_goal.pose.orientation.x = Left_Qux left_pose_goal.pose.orientation.y = Left_Quy left_pose_goal.pose.orientation.z = Left_Quz left_pose_goal.pose.orientation.w = Left_Quw left_pose_goal.pose.position.x = (Neurondata[11]-0.05)*1.48+0.053 left_pose_goal.pose.position.y = (Neurondata[9]-0.18)*1.48+0.12 left_pose_goal.pose.position.z = (Neurondata[10]-0.53)*1.48+0.47 # # 右臂 # right_pose_goal.pose.orientation.x = right_pose_goal.pose.orientation.x # right_pose_goal.pose.orientation.y = right_pose_goal.pose.orientation.y # right_pose_goal.pose.orientation.z = right_pose_goal.pose.orientation.z # right_pose_goal.pose.orientation.w = right_pose_goal.pose.orientation.w # right_pose_goal.pose.position.x = right_pose_goal.pose.position.x # right_pose_goal.pose.position.y = right_pose_goal.pose.position.y # right_pose_goal.pose.position.z = right_pose_goal.pose.position.z # # 左臂 # left_pose_goal.pose.orientation.x = left_pose_goal.pose.orientation.x # left_pose_goal.pose.orientation.y = left_pose_goal.pose.orientation.y # left_pose_goal.pose.orientation.z = left_pose_goal.pose.orientation.z # left_pose_goal.pose.orientation.w = left_pose_goal.pose.orientation.w # left_pose_goal.pose.position.x = left_pose_goal.pose.position.x # left_pose_goal.pose.position.y = left_pose_goal.pose.position.y # left_pose_goal.pose.position.z = left_pose_goal.pose.position.z # 设置动作组的两个目标点 both_arms.set_pose_target(right_pose_goal, end_effector_link="gripper_r_finger_r") both_arms.set_pose_target(left_pose_goal, end_effector_link="gripper_l_finger_r") # 规划和输出动作 traj = both_arms.plan() both_arms.execute(traj, wait=False) # # 清除路径约束 both_arms.clear_path_constraints() # 确保输出停止 both_arms.stop()
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!')
box_pose.header.frame_id = group.get_planning_frame() box_pose.pose.position.x = 0.3 box_pose.pose.position.y = -0.3 box_pose.pose.position.z = -0.25 box_pose.pose.orientation.w = 1.0 scene.add_box('box_object', box_pose, (0.4, 0.1, 0.5)) rospy.sleep(2) rospy.loginfo("Scene Objects : {}".format(scene.get_known_object_names())) # Set Path Constraint constraints = Constraints() constraints.name = "down" orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = group.get_planning_frame() orientation_constraint.link_name = group.get_end_effector_link() orientation_constraint.orientation = pose_target_1.orientation orientation_constraint.absolute_x_axis_tolerance = 3.1415 orientation_constraint.absolute_y_axis_tolerance = 0.05 orientation_constraint.absolute_z_axis_tolerance = 0.05 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) group.set_path_constraints(constraints) rospy.loginfo("Get Path Constraints:\n{}".format( group.get_path_constraints())) # Pose Target 2
def main(): # Create table obstacle rospy.init_node('arm_obstacle_demo') planning_scene = PlanningSceneInterface('base_link') planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle # planning_scene.removeCollisionObject('divider') # size_x = 0.3 # size_y = 0.01 # size_z = 0.4 # x = table_x - (table_size_x / 2) + (size_x / 2) # y = 0 # z = table_z + (table_size_z / 2) + (size_z / 2) # planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) #Orientation constraint oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 # move to pose args kwargs = { 'allowed_planning_time': 20, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } # Before moving to the first pose planning_scene.removeAttachedObject('tray') error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(1) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') # At the end of your program planning_scene.removeAttachedObject('tray') # Was working without this but now its needed planning_scene.clear()
def print_pointlist(self, point_list, future_print_status=False): startime = datetime.datetime.now() # Save original points list full_point_list = copy.deepcopy(point_list) full_point_array = np.delete(np.array(full_point_list), 2, axis=1) self.target_list = full_point_list if future_print_status: self.future_printing_status = True # Constraints pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() # joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_4' joint_constraint.position = 150 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) # Orientation constrains orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = self.group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 2 * pi orientation_constraint.absolute_y_axis_tolerance = 2 * pi orientation_constraint.absolute_z_axis_tolerance = 2 * pi orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) # Record how many points has already finished finsih_num = 0 print_num = 0 index_check = 0 while len(point_list) > 0: print('New Plan, points left:', len(point_list), point_list) # Move the robot point to first point and find the height if len(point_list) > 1: initial_plan = self.move_to_initial(point_list[1]) else: initial_plan = self.move_to_initial(point_list[0]) # joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot_state.joint_state) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) # the joints for the wheel resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) print 'Adding the first planing point, and fraction is', fraction executing_state = 0 if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan, wait=False) self.msg_print.data = True executing_state = 1 success_num += 1 elif success_num == 0: break else: # execute success plan self.msg_print.data = True self.group.execute(execute_plan, wait=False) executing_state = 1 break ## 2nd loop ## always re-plan and check for obstacle while executing waypoints executed_waypoint_index = 0 # initial value of nothing success_point_list = point_list[:success_num] print('when plan success, move_group_status:', self.move_group_execution_status, 'success_plan_number:', success_num) if executing_state == 1: success_planned_waypoint_array = np.delete(np.array( point_list[:success_num]), 2, axis=1) # print 'success planned waypoint\n', success_planned_waypoint_array print 'status', self.waypoint_execution_status while self.waypoint_execution_status != 3: if point_list == []: break if self.waypoint_execution_status == 4: # aborted state print 'stop and abort waypoint execution' self.msg_print.data = False self.group.stop() executing_state = 0 break current_ee_pose = self.group.get_current_pose().pose current_ee_position_array = np.array([ current_ee_pose.position.x, current_ee_pose.position.y ]) executed_waypoint_index = self.check_executed_waypoint_index( success_planned_waypoint_array, current_ee_position_array) # print 'executed latest index', executed_waypoint_index index_check = self.check_executed_waypoint_index( full_point_array, current_ee_position_array) self.further_printing_number = index_check print 'index:', index_check, 'way_point index', executed_waypoint_index if future_print_status == True: # Check for enclosure self.base_group.set_position_target( [0, 0, 0.05], self.base_group.get_end_effector_link()) result = self.base_group.plan() self.base_group.clear_pose_targets() if len(result.joint_trajectory.points) == 0: print('Check enclosure failed') else: print('Check enclosure successful') ## Replan to check for dynamic obstacle waypoints = [] # Add the current pose to make sure the path is smooth, get latest pose current_ee_pose = self.group.get_current_pose().pose waypoints.append(copy.deepcopy(current_ee_pose)) # discard the executed waypoints new_point_list = point_list[ executed_waypoint_index:success_num] for k in new_point_list: (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z) = k waypoints.append(copy.deepcopy(current_ee_pose)) (plan2, fraction2) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) print 'Dynamic check fraction:', fraction2 if fraction2 < 1: ## new obstacle appear # print 'executed latest index', executed_waypoint_index # print 'fraction value', fraction,'\n' print 'new obstacle appeared to be in the path' self.group.stop() self.msg_print.data = False executing_state = 0 break rospy.sleep(2) print 'status:', self.waypoint_execution_status, 'executed_index:', executed_waypoint_index, 'success_num:', success_num if self.waypoint_execution_status == 3: # waypoint successfully printed # self.print_list_visualize(point_list[:success_num]) self.rviz_visualise_marker(point_list[:success_num]) del (point_list[:success_num - 1]) elif self.waypoint_execution_status == 2 or 4: # state 2 = preempted, state 4 = aborted. # only printed partial waypoint # self.print_list_visualize(point_list[:executed_waypoint_index+1]) if executed_waypoint_index > 0: # at index 0, it might have not print the point 0-1 edge successfully. self.rviz_visualise_marker( point_list[:executed_waypoint_index + 1]) del (point_list[:executed_waypoint_index] ) # delete up till whatever is executed self.msg_print.data = False if point_list == []: rospy.sleep(2) self.group.stop() # Delete the points what already execute # if success_num > 0: # Add obstacle after printing (need to revise) self.group.set_path_constraints(None) finshtime = datetime.datetime.now() print(finshtime - startime).seconds full_point_list_x = [base[0] for base in full_point_list] full_point_list_y = [base[1] for base in full_point_list] plt3 = plt.figure("Printing result", figsize=(6, 6)) plt.plot(self.re_position_x, self.re_position_y, 'b') plt.plot(full_point_list_x, full_point_list_y, 'ro') plt.xlim(min(full_point_list_x) - 0.1, max(full_point_list_x) + 0.1) plt.ylim(min(full_point_list_y) - 0.1, max(full_point_list_y) + 0.1) plt.legend(['Printing result', 'ground truth'], fontsize=8, bbox_to_anchor=(1.0, 1)) plt.xlabel("X axis (m)") plt.ylabel("y axis (m)") plt.title('Printing result') plt.show() plt3.savefig('result.png', dpi=plt3.dpi) print('All finish, time:', (finshtime - startime).seconds)
def place(self, target_name, place_position, pre_place_distance, post_place_retreat): limit = {'dist': 0.01, 'r': 0.10, 'p': 0.10, 'y': 0.10} constraints = Constraints() oc = OrientationConstraint() oc.header.frame_id = REFERENCE_FRAME oc.link_name = 'j2s7s300_end_effector' oc.absolute_x_axis_tolerance = limit['r'] # radian oc.absolute_y_axis_tolerance = limit['p'] oc.absolute_z_axis_tolerance = limit['y'] oc.weight = 0.85 oc.orientation = place_position.pose.orientation constraints.orientation_constraints.append(deepcopy(oc)) result = False replan_times = 1 replan_state = True while replan_state and replan_times <= 5: (place_path, fraction) = self.get_path(place_position.pose, 0.01, constraints=constraints) if fraction >= 0.95: print "Pre_place_approach..." self.arm.execute(place_path) result = self.check(place_position.pose, limit) replan_state = False replan_times += 1 if result: result = False print "Placing..." self.gripper.set_named_target("Open") self.gripper.go() self.arm.detach_object(target_name) rospy.sleep(1) post_place_position = self.get_retreat_point( place_position.pose, post_place_retreat) replan_state = True replan_times = 1 while replan_state and replan_times <= 5: (retreat_path, fraction) = self.get_path(post_place_position, 0.01, constraints=constraints) if fraction > 0.8: self.arm.execute(retreat_path) result = self.check(post_place_position, limit) replan_state = False replan_times += 1 if not result: self.state = STATE.PLACE_RETREAT_ERR else: self.state = STATE.PRE_PLACE_ERR if self._server.current_goal.get_goal_status( ).status == GoalStatus.PREEMPTING: rospy.logwarn("Can't cancel task after place action start!") if result: self.state = STATE.PLACE_FINISH self.arm.clear_path_constraints()
def __init__(self): self.bridge = CvBridge() joint_state_topic = ['joint_states:=/iiwa/joint_states'] moveit_commander.roscpp_initialize(joint_state_topic) rospy.Subscriber("/iiwa/joint_states", JointState, self.State_callback) # Instantiate a RobotCommander object. This object is # an interface to the robot as a whole. self.robot = moveit_commander.RobotCommander() self.group = moveit_commander.MoveGroupCommander("manipulator") # rospy.sleep(2) # self.scene = moveit_commander.PlanningSceneInterface('/iiwa/move_group/monitored_planning_scene') # box_pose = PoseStamped() # box_pose.header.frame_id = "world" # box_pose.pose.position.x = 1.0 # box_pose.pose.orientation.w = 1.0 # self.scene.add_box("test", box_pose, size=(0.1, 0.2, 0.3)) # while not rospy.is_shutdown(): # rospy.sleep(1.0) # for k in self.scene.__dict__.keys(): # print(k, self.scene.__dict__[k]) # # print(self.scene) # print(self.scene.get_known_object_names()) # print(self.scene.get_attached_objects()) # exit() self.group.set_max_velocity_scaling_factor(0.05) self.group.set_max_acceleration_scaling_factor(0.05) current_pose = self.group.get_current_pose(end_effector_link='iiwa_link_ee').pose self._joint_efforts = 0 self._joint_vel = 0 self._joint_name = 0 self._header = None pose = PoseStamped() self.upright_constraints = Constraints() self.upright_constraints.name = "upright" orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = self.group.get_planning_frame() orientation_constraint.link_name = self.group.get_end_effector_link() pose.pose.orientation.x = 1.0 pose.pose.orientation.y = 0.0 pose.pose.orientation.z = 0.0 pose.pose.orientation.w = 0.0 orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = .7#3.0 orientation_constraint.absolute_y_axis_tolerance = .7#3.0 orientation_constraint.absolute_z_axis_tolerance = 3.1415 #orientation_constraint.absolute_z_axis_tolerance = 3.14 #ignore this axis orientation_constraint.weight = 1 self.upright_constraints.orientation_constraints.append(orientation_constraint) self.group.allow_replanning(True) self.group.allow_looking(True) workspace = [0.5,-0.3,0.15,0.7,0.2,0.25] # self.group.set_workspace(workspace) # self.group.set_path_constraints(self.upright_constraints) self.traj_num = -1 self.im_num = 0 self.MAX_PATH_LENGTH = 15
def main(): """ Main Script """ # Make sure that you've looked at and understand path_planner.py before starting ## TF CODE tfBuffer = tf2_ros.Buffer() tfListener = tf2_ros.TransformListener(tfBuffer) ## TF CODE planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned limb = intera_interface.Limb("right") control = Controller(Kp, Kd, Ki, Kw, limb) ## ## Add the obstacle to the planning scene here ## X = 0.40 Y = 1.20 Z = 0.10 Xp = 0.5 Yp = 0.00 Zp = -0.15 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa #planner.add_box_obstacle([X,Y,Z], "wall", pose) #Wall 2 X = 0.40 Y = 0.10 Z = 0.30 Xp = 0.5 Yp = -0.15 Zp = 0.05 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa #planner.add_box_obstacle([X,Y,Z], "wall2", pose) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART 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.05 orien_const.absolute_z_axis_tolerance = 0.05 orien_const.weight = 1.0 waypoints = list() # Section to add waypoints # step_size = size between x position changes # final_length = final x position #GOAL Points # goal_1.pose.position.x = 0.585 # goal_1.pose.position.y = 0.156 # goal_1.pose.position.z = -0.138 # goal_1.pose.orientation.x = 0 # goal_1.pose.orientation.y = 0.707 # goal_1.pose.orientation.z = 0 # goal_1.pose.orientation.w = 0.707 #GOAL Points def moveUp(planner, control, waypoints, value_of_step): control._probing = 1 #STARTOF WORKING CODE # Z MUST MOVE UP TWO BLOCK # Z = 0.025 #control._velocity_scalar = 0.5 jenga = 0 while (jenga != 1): step_size = 0.0005 #0.0025 FINAL_X = 0.785 # offset = 0.14 tf_px = 0.585 tf_py = 0.156 tf_pz = -0.138 tf_rx = 0 tf_ry = 0.707 tf_rz = 0 tf_rw = 0.707 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w goal_x = tf_px goal_y = tf_py goal_z = tf_pz except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue offset = 0.14 FINAL_Z = goal_z + value_of_step # + offsett z = tf_pz #+ offset #print("TF'd zero") update = 0 execute = 0 i = 0 STOP = 0 while ((z < FINAL_Z or update == 0) and (i < 3)): if (update == 0): try: trans = tfBuffer.lookup_transform( 'base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w z = tf_pz # + offset except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue update = 1 execute = 0 y = tf_py x = tf_px + offset if (y < goal_y - 0.05): y = y + step_size if (y > goal_y + 0.05): y = y - step_size #print("TF'd while") goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) z = z + step_size if (z >= FINAL_Z): execute = 1 #print("added waypoint") if (execute == 1): while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break #STOP = control._flag waypoints = list() update = 0 #print("EXEcute") #print(i) i = i + 1 #print("END OF LOOP") jenga = 1 control._probing = 0 # END OF WORKING CODE def moveDown(planner, control, waypoints): control._probing = 1 #STARTOF WORKING CODE # Z MUST MOVE UP TWO BLOCK # Z = 0.025 value_of_step = 0.033 #control._velocity_scalar = 0.5 jenga = 0 while (jenga != 1): step_size = 0.0005 #0.0025 FINAL_X = 0.785 # offset = 0.14 tf_px = 0.585 tf_py = 0.156 tf_pz = -0.138 tf_rx = 0 tf_ry = 0.707 tf_rz = 0 tf_rw = 0.707 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w goal_x = tf_px goal_y = tf_py goal_z = tf_pz except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue offset = 0.14 FINAL_Z = goal_z - value_of_step # + offsett z = tf_pz #+ offset #print("TF'd zero") update = 0 execute = 0 i = 0 STOP = 0 while ((z > FINAL_Z or update == 0) and (i < 3)): if (update == 0): try: trans = tfBuffer.lookup_transform( 'base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w z = tf_pz # + offset except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue update = 1 execute = 0 y = tf_py x = tf_px + offset if (y < goal_y - 0.05): y = y + step_size if (y > goal_y + 0.05): y = y - step_size #print("TF'd while") goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) z = z - step_size if (z <= FINAL_Z): execute = 1 #print("added waypoint") if (execute == 1): while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break #STOP = control._flag waypoints = list() update = 0 #print("EXEcute") #print(i) i = i + 1 #print("END OF LOOP") jenga = 1 control._probing = 0 # END OF WORKING CODE def moveLeft(planner, control, waypoints, value_of_step): control._probing = 1 #STARTOF WORKING CODE # Z MUST MOVE UP TWO BLOCK # Z = 0.025 #value_of_step = 0.014 #control._velocity_scalar = 0.5 jenga = 0 while (jenga != 1): step_size = 0.0005 #0.0025 FINAL_X = 0.785 # offset = 0.14 tf_px = 0.585 tf_py = 0.156 tf_pz = -0.138 tf_rx = 0 tf_ry = 0.707 tf_rz = 0 tf_rw = 0.707 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w goal_x = tf_px goal_y = tf_py goal_z = tf_pz #print(goal_z) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue offset = 0.14 FINAL_Y = goal_y + value_of_step # + offsett y = tf_py #+ offset update = 0 execute = 0 i = 0 STOP = 0 while ((y < FINAL_Y or update == 0) and (i < 3)): if (update == 0): try: trans = tfBuffer.lookup_transform( 'base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w y = tf_py # + offset except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue update = 1 execute = 0 x = tf_px + offset z = tf_pz #print("TF'd while") goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) y = y + step_size if (y >= FINAL_Y): execute = 1 #print("added waypoint") if (execute == 1): while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break #STOP = control._flag waypoints = list() update = 0 #print("EXEcute") #print(i) i = i + 1 #print("END OF LOOP") jenga = 1 control._probing = 0 # END OF WORKING CODE def moveRight(planner, control, waypoints): control._probing = 1 #STARTOF WORKING CODE # Z MUST MOVE UP TWO BLOCK # Z = 0.025 value_of_step = 0.014 #control._velocity_scalar = 0.5 jenga = 0 while (jenga != 1): step_size = 0.0005 #0.0025 FINAL_X = 0.785 # offset = 0.14 tf_px = 0.585 tf_py = 0.156 tf_pz = -0.138 tf_rx = 0 tf_ry = 0.707 tf_rz = 0 tf_rw = 0.707 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w goal_x = tf_px goal_y = tf_py goal_z = tf_pz except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue offset = 0.14 FINAL_Y = goal_y - value_of_step y = tf_py #+ offset #print("TF'd zero") update = 0 execute = 0 i = 0 STOP = 0 while ((y > FINAL_Y or update == 0) and (i < 3)): if (update == 0): try: trans = tfBuffer.lookup_transform( 'base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w y = tf_py # + offset except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue update = 1 execute = 0 z = tf_pz x = tf_px + offset #print("TF'd while") goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) y = y - step_size if (y <= FINAL_Y): execute = 1 #print("added waypoint") if (execute == 1): while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break #STOP = control._flag print("LAST WAYPOINT") print(waypoints[-1].position) waypoints = list() update = 0 #print("EXEcute") #print(i) i = i + 1 #print("END OF LOOP") jenga = 1 control._probing = 0 # END OF WORKING CODE def probe(planner, control, waypoints): control._probing = 1 value_of_step = 0.004 step_size = 0.0005 #0.0025 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 jenga = 0 while (jenga != 1): FINAL_X = 0.785 # offset = 0.14 tf_px = 0.585 tf_py = 0.156 tf_pz = -0.138 tf_rx = 0 tf_ry = 0.707 tf_rz = 0 tf_rw = 0.707 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w goal_x = tf_px goal_y = tf_py goal_z = tf_pz except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue offset = 0.14 FINAL_X = goal_x + offset + value_of_step x = tf_px + offset update = 0 execute = 0 STOP = 0 while ((x < FINAL_X or update == 0)): if (update == 0): try: trans = tfBuffer.lookup_transform( 'base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w x = tf_px + offset except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue update = 1 execute = 0 y = tf_py z = tf_pz if (y < goal_y - 0.05): y = y + step_size if (y > goal_y + 0.05): y = y - step_size if (z < goal_z - 0.05): z = z + step_size if (z > goal_z + 0.05): z = z - step_size goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.. #print("added a point") waypoints.append(goal_1.pose) x = x + step_size if (x >= FINAL_X): execute = 1 if (execute == 1): while not rospy.is_shutdown(): try: #print("Executing Probe") plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break jenga = 1 waypoints = waypoints[::-1] goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = goal_x + offset goal_1.pose.position.y = goal_y goal_1.pose.position.z = goal_z goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.. #waypoints.append(goal_1.pose) while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break print("PROBE TEST") print(min(list(map(float, control._sensor_data)))) control._poke_data.append(min(list(map(float, control._sensor_data)))) control._sensor_data = list() control._scalar_data = list() control._probing = 0 def zero_spot(planner, control, waypoints): control._probing = 1 jenga = 0 while (jenga != 1): step_size = 0.0005 #0.0025 FINAL_X = 0.585 FINAL_Y = 0.156 FINAL_Z = -0.138 offset = 0.14 tf_px = 0.585 tf_py = 0.156 tf_pz = -0.138 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 x_done = 0 y_done = 0 z_done = 0 try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue offset = 0.14 x = tf_px + offset y = tf_py z = tf_pz update = 0 execute = 0 i = 0 while (((x_done == 0 or y_done == 0 or z_done == 0) or update == 0) and (i < 3)): if (update == 0): try: trans = tfBuffer.lookup_transform( 'base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w x = tf_px + offset y = tf_py z = tf_pz except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue update = 1 execute = 0 #print("TF'd while") goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) print(x) if (x < goal_x + offset - 0.05): x = x + step_size print("add") elif (x > goal_x + offset + 0.05): x = x - step_size print("sub") else: x_done = 1 print("xdone") if (y < goal_y - 0.05): y = y + step_size elif (y > goal_y + 0.05): y = y - step_size else: y_done = 1 print("ydone") if (z < goal_z - 0.05): z = z + step_size elif (z > goal_z + 0.05): z = z - step_size else: z_done = 1 print("zdone") if (x_done == 1 and y_done == 1 and z_done == 1): execute = 1 print("execute done") if (execute == 1): while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) print("EXECUTED ZERO") if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break #STOP = control._flag waypoints = list() update = 0 #print("EXEcute") #print(i) i = i + 1 #print("END OF LOOP") jenga = 1 control._probing = 0 # END OF WORKING CODE while not rospy.is_shutdown(): value_of_step_up = 0.023 value_of_step_left = 0.0125 story = 0 while (story < 1): control._probing = 1 ## ZERO THE ROBOT ## goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = 0.585 goal_1.pose.position.y = 0.156 goal_1.pose.position.z = -0.138 goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break waypoints = list() ## ZERO THE ROBOT ## print("ROBOT ZEROED") #ospy.sleep(2) bump = 0 while (story - bump > 0): if (bump > 2): value_of_step_up = 0.033 else: value_of_step_up = 0.025 moveUp(planner, control, waypoints, value_of_step_up) waypoints = list() rospy.sleep(2) bump += 1 probe(planner, control, waypoints) waypoints = list() rospy.sleep(2) moveLeft(planner, control, waypoints, value_of_step_left) waypoints = list() rospy.sleep(2) probe(planner, control, waypoints) waypoints = list() rospy.sleep(2) moveLeft(planner, control, waypoints, value_of_step_left) waypoints = list() rospy.sleep(2) probe(planner, control, waypoints) waypoints = list() rospy.sleep(2) story += 1 control._probing = 1 ## ZERO THE ROBOT ## goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = 0.585 goal_1.pose.position.y = 0.156 goal_1.pose.position.z = -0.138 goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break waypoints = list() ## ZERO THE ROBOT ## print("ROBOT ZEROED") #rospy.sleep(2) control._probing = 0 control._trigger = 0 control._flag = 0 control._zero = 0 #control._velocity_scalar = 0.5 step_size = 0.0005 #0.0025 FINAL_X = 0.785 # offset = 0.14 tf_px = 0.585 tf_py = 0.156 tf_pz = -0.138 tf_rx = 0 tf_ry = 0.707 tf_rz = 0 tf_rw = 0.707 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w goal_x = tf_px goal_y = tf_py goal_z = tf_pz except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue offset = 0.14 FINAL_X = goal_x + offset + 0.2 x = tf_px + offset print("TF'd zero") update = 0 execute = 0 i = 0 STOP = 0 while ((x < FINAL_X or update == 0) and (i < 5)): if (update == 0): try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w x = tf_px + offset except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue update = 1 execute = 0 y = tf_py z = tf_pz if (y < goal_y - 0.05): y = y + step_size if (y > goal_y + 0.05): y = y - step_size if (z < goal_z - 0.05): z = z + step_size if (z > goal_z + 0.05): z = z - step_size #print("TF'd while") goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) x = x + step_size if (x >= FINAL_X): execute = 1 #print("added waypoint") if (execute == 1): while not rospy.is_shutdown(): try: control._probing = 0 plan = planner.plan_to_pose(waypoints, list()) print("TAKING BLOCK OUT EXECUTE") if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break #STOP = control._flag waypoints = list() update = 0 #print("EXEcute") #print(i) i = i + 1 print("END OF LOOP") control._probing = 0 control._trigger = 0 control._flag = 0 control._zero = 1 ## ZERO THE ROBOT ## goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = 0.585 goal_1.pose.position.y = 0.156 goal_1.pose.position.z = -0.138 goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break waypoints = list() ## ZERO THE ROBOT ## print("ROBOT ZEROED") #ospy.sleep(2) control._probing = 0 control._trigger = 0 control._flag = 0 control._zero = 0 moveLeft(planner, control, waypoints, value_of_step_left) waypoints = list() rospy.sleep(2) moveLeft(planner, control, waypoints, value_of_step_left) waypoints = list() rospy.sleep(2) #control._velocity_scalar = 0.5 step_size = 0.0005 #0.0025 FINAL_X = 0.785 # offset = 0.14 tf_px = 0.585 tf_py = 0.156 tf_pz = -0.138 tf_rx = 0 tf_ry = 0.707 tf_rz = 0 tf_rw = 0.707 goal_x = 0.585 goal_y = 0.156 goal_z = -0.138 try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w goal_x = tf_px goal_y = tf_py goal_z = tf_pz except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue offset = 0.14 FINAL_X = goal_x + offset + 0.2 x = tf_px + offset print("TF'd zero") update = 0 execute = 0 i = 0 STOP = 0 while ((x < FINAL_X or update == 0) and (i < 5)): if (update == 0): try: trans = tfBuffer.lookup_transform('base', 'right_gripper_base', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w x = tf_px + offset except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue update = 1 execute = 0 y = tf_py z = tf_pz if (y < goal_y - 0.05): y = y + step_size if (y > goal_y + 0.05): y = y - step_size if (z < goal_z - 0.05): z = z + step_size if (z > goal_z + 0.05): z = z - step_size #print("TF'd while") goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) x = x + step_size if (x >= FINAL_X): execute = 1 #print("added waypoint") if (execute == 1): while not rospy.is_shutdown(): try: control._probing = 0 plan = planner.plan_to_pose(waypoints, list()) print("TAKING BLOCK OUT EXECUTE") if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break #STOP = control._flag waypoints = list() update = 0 #print("EXEcute") #print(i) i = i + 1 print("END OF LOOP") control._probing = 0 control._trigger = 0 control._flag = 0 control._zero = 1 ## ZERO THE ROBOT ## goal_1 = PoseStamped() goal_1.header.frame_id = "base" goal_1.pose.position.x = 0.585 goal_1.pose.position.y = 0.156 goal_1.pose.position.z = -0.138 goal_1.pose.orientation.x = 0 # -0.026 goal_1.pose.orientation.y = 0.707 # 0.723 goal_1.pose.orientation.z = 0 # -0.052 goal_1.pose.orientation.w = 0.707 # 0.689 waypoints.append(goal_1.pose) while not rospy.is_shutdown(): try: plan = planner.plan_to_pose(waypoints, list()) if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break waypoints = list() ## ZERO THE ROBOT ## print("ROBOT ZEROED") #ospy.sleep(2) control._probing = 0 control._trigger = 0 control._flag = 0 control._zero = 0 #waypoints = list() #zero_spot(planner, control, waypoints) #waypoints = list() #rospy.sleep(2) #waypoints = list() #rospy.sleep(2) #zero_spot(planner, control, waypoints) #waypoints = list() #rospy.sleep(2) # #probe(planner, control, waypoints) # waypoints = list() # moveUp(planner, control, waypoints) # waypoints = list() # #probe(planner, control, waypoints) # waypoints = list() # moveLeft(planner, control, waypoints) # waypoints = list() # #probe(planner, control, waypoints) # waypoints = list() # moveLeft(planner, control, waypoints) # waypoints = list() # #probe(planner, control, waypoints) # waypoints = list() #plt.plot(control._sensor_data) s = list(map(float, control._sensor_data)) scales = list(map(float, control._scalar_data)) goal_line = list(map(float, control._goal_list)) #s = list(map(float, control._debug)) #control._sensor_data = sorted(control._sensor_data) #print(min(s)) #print(max(s)) plt.subplot(211) plt.plot(s) plt.plot(goal_line) plt.axis([0, len(control._sensor_data), min(s), max(s)]) plt.ylabel('Sensor Data') plt.subplot(212) plt.plot(scales) plt.show() # PRINT GRID control._sensor_data = list() control._scalar_data = list() control._goal_list = list() waypoints = list() control._velocity_scalar = 1
def right_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm right_arm = self.right_arm # 获取当前末端执行器位置姿态 pose_goal = right_arm.get_current_pose().pose # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55 : right_joint_const.position = 0.024 else: right_joint_const.position = 0 right_joint_const.weight = 1.0 # 限制1轴转动 right_joint1_const = JointConstraint() right_joint1_const.joint_name = "yumi_joint_1_r" right_joint1_const.position = 0 right_joint1_const.tolerance_above = 120 right_joint1_const.tolerance_below = 0 right_joint1_const.weight = 1.0 # 限制2轴转动 right_joint2_const = JointConstraint() right_joint2_const.joint_name = "yumi_joint_2_r" right_joint2_const.position = 0 right_joint2_const.tolerance_above = 0 right_joint2_const.tolerance_below = 150 right_joint2_const.weight = 1.0 # 限制3轴转动 right_joint3_const = JointConstraint() right_joint3_const.joint_name = "yumi_joint_3_r" right_joint3_const.position = 0 right_joint3_const.tolerance_above = 35 right_joint3_const.tolerance_below = 55 right_joint3_const.weight = 1.0 # 限制4轴转动 right_joint4_const = JointConstraint() right_joint4_const.joint_name = "yumi_joint_4_r" right_joint4_const.position = 0 right_joint4_const.tolerance_above = 60 right_joint4_const.tolerance_below = 75 right_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_r" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 right_joint6_const = JointConstraint() right_joint6_const.joint_name = "yumi_joint_6_r" right_joint6_const.position = 0 right_joint6_const.tolerance_above = 10 right_joint6_const.tolerance_below = 35 right_joint6_const.weight = 1.0 # 限制7轴转动 right_joint7_const = JointConstraint() right_joint7_const.joint_name = "yumi_joint_7_r" right_joint7_const.position = -10 right_joint7_const.tolerance_above = 0 right_joint7_const.tolerance_below = 160 right_joint7_const.weight = 1.0 # 限制末端位移 right_position_const = PositionConstraint() right_position_const.header = Header() right_position_const.link_name = "gripper_r_joint_r" right_position_const.target_point_offset.x = 0.5 right_position_const.target_point_offset.y = -0.5 right_position_const.target_point_offset.z = 1.0 right_position_const.weight = 1.0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = pose_goal.orientation right_orientation_const.link_name = "gripper_r_finger_r" right_orientation_const.absolute_x_axis_tolerance = 0.50 right_orientation_const.absolute_y_axis_tolerance = 0.25 right_orientation_const.absolute_z_axis_tolerance = 0.50 right_orientation_const.weight = 100 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const] # consts.orientation_constraints = [right_orientation_const] # consts.position_constraints = [right_position_const] right_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Right_Qux pose_goal.orientation.y = Right_Quy pose_goal.orientation.z = Right_Quz pose_goal.orientation.w = Right_Quw pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053 pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12 pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47 right_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = 0.1644 # pose_goal.orientation.y = 0.3111 # pose_goal.orientation.z = 0.9086 # pose_goal.orientation.w = 0.2247 # pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053 # pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12 # pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47 # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = right_arm.plan() right_arm.execute(traj, wait=False) # 动作完成后清除目标信息 right_arm.clear_pose_targets() # 清除路径约束 right_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 right_arm.stop()
def left_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm left_arm = self.left_arm # 获取当前末端执行器位置姿态 pose_goal = left_arm.get_current_pose().pose # 限制末端夹具运动 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32 : left_joint_const.position = 0.024 else: left_joint_const.position = 0 left_joint_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = -0.5 left_position_const.target_point_offset.z = 1.0 left_position_const.weight = 1.0 # # 限制1轴转动 left_joint1_const = JointConstraint() left_joint1_const.joint_name = "yumi_joint_1_l" left_joint1_const.position = 0 left_joint1_const.tolerance_above = 1.76 left_joint1_const.tolerance_below = 0 left_position_const.weight = 1.0 # 限制2轴转动 left_joint2_const = JointConstraint() left_joint2_const.joint_name = "yumi_joint_2_l" left_joint2_const.position = 0 left_joint2_const.tolerance_above = 0 left_joint2_const.tolerance_below = 150 left_joint2_const.weight = 1.0 # 限制3轴转动 left_joint3_const = JointConstraint() left_joint3_const.joint_name = "yumi_joint_3_l" left_joint3_const.position = 0 left_joint3_const.tolerance_above = 35 left_joint3_const.tolerance_below = 55 left_joint3_const.weight = 1.0 # 限制4轴转动 left_joint4_const = JointConstraint() left_joint4_const.joint_name = "yumi_joint_4_l" left_joint4_const.position = 0 left_joint4_const.tolerance_above = 60 left_joint4_const.tolerance_below = 75 left_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_l" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 left_joint6_const = JointConstraint() left_joint6_const.joint_name = "yumi_joint_6_l" left_joint6_const.position = 0 left_joint6_const.tolerance_above = 10 left_joint6_const.tolerance_below = 35 left_joint6_const.weight = 1.0 # 限制7轴转动 left_joint7_const = JointConstraint() left_joint7_const.joint_name = "yumi_joint_7_l" left_joint7_const.position = -10 left_joint7_const.tolerance_above = 0 left_joint7_const.tolerance_below = 160 left_joint7_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = 0.25 left_position_const.target_point_offset.z = 0.5 left_position_const.weight = 1.0 # 添加末端姿态约束: left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = pose_goal.orientation left_orientation_const.link_name = "gripper_l_finger_r" left_orientation_const.absolute_x_axis_tolerance = 0.5 left_orientation_const.absolute_y_axis_tolerance = 0.25 left_orientation_const.absolute_z_axis_tolerance = 0.5 left_orientation_const.weight = 1 # 施加全约束 consts = Constraints() consts.joint_constraints = [left_joint_const] # consts.orientation_constraints = [left_orientation_const] # consts.position_constraints = [left_position_const] left_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Left_Qux pose_goal.orientation.y = Left_Quy pose_goal.orientation.z = Left_Quz pose_goal.orientation.w = Left_Quw pose_goal.position.x = (Neurondata[11]-0.05)*1.48+0.053 pose_goal.position.y = (Neurondata[9]-0.18)*1.48+0.12 pose_goal.position.z = (Neurondata[10]-0.53)*1.48+0.47 left_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # left_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = left_arm.plan() left_arm.execute(traj, wait=False) # 动作完成后清除目标信息 left_arm.clear_pose_targets() # 清除路径约束 left_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 left_arm.stop()
def main(): global prev_msg global sec_pre plandraw = PathPlanner('right_arm') # plandraw.gripper_close() # rospy.sleep(10) # plandraw.grip?per_op plandraw.start_position() ## BOX box_size = np.array([2.4, 2.4, 0.1]) box_pose = PoseStamped() box_pose.header.stamp = rospy.Time.now() box_pose.header.frame_id = "base" box_pose.pose.position.x = 0 box_pose.pose.position.y = 0 box_pose.pose.position.z = -0.4 box_pose.pose.orientation.x = 0.00 box_pose.pose.orientation.y = 0.00 box_pose.pose.orientation.z = 0.00 box_pose.pose.orientation.w = 1.00 plandraw.add_box_obstacle(box_size, "box1", box_pose) box_size2 = np.array([2.4, 2.4, 0.1]) box_pose2 = PoseStamped() box_pose2.header.stamp = rospy.Time.now() box_pose2.header.frame_id = "base" box_pose2.pose.position.x = 0 box_pose2.pose.position.y = 0 box_pose2.pose.position.z = 1 box_pose2.pose.orientation.x = 0.00 box_pose2.pose.orientation.y = 0.00 box_pose2.pose.orientation.z = 0.00 box_pose2.pose.orientation.w = 1.00 plandraw.add_box_obstacle(box_size2, "box2", box_pose2) orien_const = OrientationConstraint() orien_const.link_name = "right_gripper_tip" 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 def set_use_pen(pen_id, goal_1): if pen_id == 0: # Blue The innar one goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -0.9848078 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = -0.1736482 goal_1.pose.position.x -= 0.003 goal_1.pose.position.y -= 0.013 goal_1.pose.position.z -= 0.011 if pen_id == 1: 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 goal_1.pose.position.x -= 0.014 goal_1.pose.position.y -= 0.002 goal_1.pose.position.z -= 0.017 if pen_id == 2: goal_1.pose.orientation.x = 0.0 goal_1.pose.orientation.y = -0.9848078 goal_1.pose.orientation.z = 0.0 goal_1.pose.orientation.w = 0.1736482 goal_1.pose.position.x -= 0.028 goal_1.pose.position.y -= 0.003 goal_1.pose.position.z -= 0.010 # if pen_id == 0: # goal_1.pose.orientation.x = 0.0 # goal_1.pose.orientation.y = -0.925877 # goal_1.pose.orientation.z = 0.0 # goal_1.pose.orientation.w = -0.360095 # if pen_id == 1: # goal_1.pose.orientation.x = 0.0 # goal_1.pose.orientation.y = -0.9994 # goal_1.pose.orientation.z = 0.0 # goal_1.pose.orientation.w = 0.005035 # if pen_id == 2: # goal_1.pose.orientation.x = 0.0 # goal_1.pose.orientation.y = -0.974507 # goal_1.pose.orientation.z = 0.0 # goal_1.pose.orientation.w = 0.222739 waypoints = [] while not rospy.is_shutdown(): #raw_input("~~~~~~~~~~~~!!!!!!!!!!!!") while not rospy.is_shutdown(): try: while len(queue): # print(len(queue)) cur = queue.popleft() x, y, z = cur.position_x, cur.position_y, cur.position_z x += 0.002 # ada different coordinate z -= 0.103 z += 0.037 # z += 0.2 x -= 0.013 y += 0.010 z += 0.005 x += 0.12 if cur.status_type != "edge_grad": # ti bi !!!!! luo bi !!!! if cur.status_type == "starting": print("start") # waypoints = [] goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z + 0.12 #Orientation as a quaternion # [0.766, -0.623, 0.139, -0.082] # [-0.077408, 0.99027, -0.024714, ] set_use_pen(cur.pen_type, goal_1) waypoints.append(copy.deepcopy(goal_1.pose)) goal_1.pose.position.z -= 0.12 waypoints.append(copy.deepcopy(goal_1.pose)) # plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints) # if not plandraw.execute_plan(plan): # raise Exception("Starting execution failed") # else: # queue.pop(0) elif cur.status_type == "next_point": # print("next") goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z #Orientation as a quaternion # goal_1.pose.orientation.x = 0.459962 # goal_1.pose.orientation.y = -0.7666033 # goal_1.pose.orientation.z = 0.0 # goal_1.pose.orientation.w = -0.4480562 set_use_pen(cur.pen_type, goal_1) # waypoints = [] waypoints.append(copy.deepcopy(goal_1.pose)) # plan = plandraw.plan_to_pose(goal_1, [orien_const], waypoints) # if not plandraw.execute_plan(plan): # raise Exception("Execution failed, point is ", cur) # else: # queue.pop(0) elif cur.status_type == "ending": print("ppppppp ", cur) # mmm = plandraw.get_cur_pos().pose goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = x goal_1.pose.position.y = y goal_1.pose.position.z = z + 0.12 #Orientation as a quaternion set_use_pen(1, goal_1) # waypoints = [] waypoints.append(copy.deepcopy(goal_1.pose)) plan = plandraw.plan_to_pose(goal_1, [], waypoints) waypoints = [] # queue.pop(0) if not plandraw.execute_plan(plan): raise Exception("Execution failed") print("ti bi") # rospy.sleep(5) #raw_input("Press <Enter> to move next!!!") except Exception as e: print e else: #print("lllllllllllllllllllll") break
def probe(self, nx, ny, dx, dy): #Initialize arms robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() right_arm = moveit_commander.MoveGroupCommander('right_arm') right_arm.set_planner_id('RRTConnectkConfigDefault') right_arm.set_planning_time(15) # Set constraints rospy.sleep(2) # Assuming you're facing the wall, looking at the robot # And the robot's computer is to your left # Then Wall 1 is the wall on "your" right # Wall 2 is the wall to "your" left # Wall 3 is the wall in the back #raw_input("press any key to add wall 1") p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0.7 p.pose.position.y = 0. p.pose.position.z = 0. scene.add_box("wall1", p, (0.1, 5, 5)) #raw_input("press any key to add wall 2") p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = -1 p.pose.position.y = 0. p.pose.position.z = 0. scene.add_box("wall2", p, (0.1, 5, 5)) #raw_input("press any key to add wall 3") p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0 p.pose.position.y = 0.7 p.pose.position.z = 0. scene.add_box("wall3", p, (5, 0.1, 5)) #raw_input("press any key to add patient") p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = self.table_x p.pose.position.y = self.table_y p.pose.position.z = self.table_z / 2.0 scene.add_box("patient", p, (0.3, 0.3, self.table_z / 2.0)) 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 = 50 orien_const.weight = .5 consts = Constraints() consts.orientation_constraints = [orien_const] right_arm.set_path_constraints(consts) ans = {} # start with empty dictionary for i in range(-nx, nx + 1): for j in range(-nx, nx + 1): ans[(i, j)] = self.poke_at(right_arm, self.table_x + i * dx, self.table_y + j * dy) return ans
def moveToPose(self, pose_stamped, gripper_frame, tolerance=0.01, wait=True, **kwargs): # Check arguments supported_args = ("max_velocity_scaling_factor", "planner_id", "planning_time", "plan_only", "start_state") for arg in kwargs.keys(): if not arg in supported_args: rospy.loginfo("moveToPose: unsupported argument: %s", arg) # Create goal g = MoveGroupGoal() pose_transformed = self._listener.transformPose( self._fixed_frame, pose_stamped) # 1. fill in request workspace_parameters # 2. fill in request start_state try: g.request.start_state = kwargs["start_state"] except KeyError: g.request.start_state.is_diff = True # 3. fill in request goal_constraints c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self._fixed_frame c1.position_constraints[0].link_name = gripper_frame b = BoundingVolume() s = SolidPrimitive() s.dimensions = [tolerance * tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(pose_transformed.pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self._fixed_frame c1.orientation_constraints[ 0].orientation = pose_transformed.pose.orientation c1.orientation_constraints[0].link_name = gripper_frame c1.orientation_constraints[0].absolute_x_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_y_axis_tolerance = tolerance c1.orientation_constraints[0].absolute_z_axis_tolerance = tolerance c1.orientation_constraints[0].weight = 1.0 g.request.goal_constraints.append(c1) # 4. fill in request path constraints # 5. fill in request trajectory constraints # 6. fill in request planner id try: g.request.planner_id = kwargs["planner_id"] except KeyError: if self.planner_id: g.request.planner_id = self.planner_id # 7. fill in request group name g.request.group_name = self._group # 8. fill in request number of planning attempts try: g.request.num_planning_attempts = kwargs["num_attempts"] except KeyError: g.request.num_planning_attempts = 1 # 9. fill in request allowed planning time try: g.request.allowed_planning_time = kwargs["planning_time"] except KeyError: g.request.allowed_planning_time = self.planning_time # Fill in velocity scaling factor try: g.request.max_velocity_scaling_factor = kwargs[ "max_velocity_scaling_factor"] except KeyError: pass # do not fill in at all # 10. fill in planning options diff g.planning_options.planning_scene_diff.is_diff = True g.planning_options.planning_scene_diff.robot_state.is_diff = True # 11. fill in planning options plan only try: g.planning_options.plan_only = kwargs["plan_only"] except KeyError: g.planning_options.plan_only = self.plan_only # 12. fill in other planning options g.planning_options.look_around = False g.planning_options.replan = False # 13. send goal self._action.send_goal(g) if wait: self._action.wait_for_result() return self._action.get_result() else: return None
def main(): """ Main Script """ #=================================================== # Code to add gripper #rospy.init_node('gripper_test') #Set up the right gripper right_gripper = robot_gripper.Gripper('right_gripper') #Calibrate the gripper (other commands won't work unless you do this first) # print('Calibrating...') # right_gripper.calibrate() # rospy.sleep(2.0) #Close the right gripper to hold publisher # print('Closing...') # right_gripper.close() # rospy.sleep(1.0) #=================================================== # Make sure that you've looked at and understand path_planner.py before starting planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5 ]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned limb = intera_interface.Limb("right") control = Controller(Kp, Kd, Ki, Kw, limb) ## ## Add the obstacle to the planning scene here ## #Tower #TODO: make wrt to sawyer (currently wrt to ar tag) X = 0.075 Y = 0.075 Z = 0.045 Xp = 0.0 Yp = -0.0425 Zp = 0.0225 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 # pose = PoseStamped() # pose.header.stamp = rospy.Time.now() # #TODO: is this the right frame name? # pose.header.frame_id = "ar_marker_1" # pose.pose.position.x = Xp # pose.pose.position.y = Yp # pose.pose.position.z = Zp # pose.pose.orientation.x = Xpa # pose.pose.orientation.y = Ypa # pose.pose.orientation.z = Zpa # pose.pose.orientation.w = Wpa # planner.add_box_obstacle([X,Y,Z], "tower", pose) #Table (currently wrt ar tag) X = 1 Y = 1 Z = .005 Xp = 0 Yp = 0 Zp = 0 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() #TODO: Is this the correct frame name? pose.header.frame_id = "ar_marker_1" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa planner.add_box_obstacle([X, Y, Z], "table", pose) # #Create a path constraint for the arm # #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART orien_const = OrientationConstraint() orien_const.link_name = "right_gripper_tip" #changed from "base" frame_id orien_const.header.frame_id = "ar_marker_1" orien_const.orientation.w = 1 orien_const.absolute_x_axis_tolerance = 0.001 orien_const.absolute_y_axis_tolerance = 0.001 orien_const.absolute_z_axis_tolerance = 0.001 orien_const.weight = 20.0 rospy.sleep(1.0) while not rospy.is_shutdown(): while not rospy.is_shutdown(): try: #currently wrt ar tag goal_1 = PoseStamped() goal_1.header.frame_id = "ar_marker_1" #x, y, and z position goal_1.pose.position.x = 0.0 goal_1.pose.position.y = -0.0425 goal_1.pose.position.z = 0.0225 #Orientation as a quaternion goal_1.pose.orientation.x = 0 goal_1.pose.orientation.y = np.pi / 2 goal_1.pose.orientation.z = 0 goal_1.pose.orientation.w = 0 plan = planner.plan_to_pose(goal_1, list()) # if not planner.execute_plan(plan): if not control.execute_path(plan): raise Exception("Execution failed") except Exception as e: print e else: break
def pick(self, target_name, grasp_position, pre_grasp_distance, post_grasp_retreat): pre_grasp_posture = JointTrajectory() grasp_posture = JointTrajectory() pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) limit = {'dist': 0.01, 'r': 0.15, 'p': 0.15, 'y': 0.15} constraints = Constraints() oc = OrientationConstraint() oc.header.frame_id = REFERENCE_FRAME oc.link_name = 'j2s7s300_end_effector' oc.absolute_x_axis_tolerance = limit['r'] # radian oc.absolute_y_axis_tolerance = limit['p'] oc.absolute_z_axis_tolerance = limit['y'] oc.weight = 1.0 oc.orientation = grasp_position.pose.orientation constraints.orientation_constraints.append(deepcopy(oc)) result = False replan_times = 1 replan_state = True while replan_state and replan_times <= 5: (pre_grasp_path, fraction) = self.get_path(grasp_position.pose, 0.01, constraints=constraints) if self._server.current_goal.get_goal_status( ).status == GoalStatus.PREEMPTING: self.state = STATE.STOP return if fraction >= 0.92: print "Pre_grasp_approach..." self.arm.execute(pre_grasp_path) result = self.check(grasp_position.pose, limit) replan_state = False replan_times += 1 if result: result = False if self.arm.attach_object( target_name, self.arm.get_end_effector_link(), [ "j2s7s300_end_effector", "j2s7s300_link_finger_1", "j2s7s300_link_finger_tip_1", "j2s7s300_link_finger_2", "j2s7s300_link_finger_tip_2", "j2s7s300_link_finger_3", "j2s7s300_link_finger_tip_3", "j2s7s300_joint_finger_1", "j2s7s300_joint_finger_2", "j2s7s300_joint_finger_3" ]): rospy.logwarn("Attach request sent successfully!") else: raise Exception("Can't send attach request!") print "Grasping..." self.gripper.set_named_target("Close") if self._server.current_goal.get_goal_status( ).status == GoalStatus.PREEMPTING: self.arm.detach_object(target_name) self.back_to_init_pose() self.state = STATE.STOP return self.gripper.go() rospy.sleep(1) post_grasp_position = self.get_retreat_point( grasp_position.pose, post_grasp_retreat) print "post_grasp_position: %s\n" % post_grasp_position replan_times = 1 replan_state = True while replan_state and replan_times <= 5: (retreat_path, fraction) = self.get_path(post_grasp_position, 0.005, constraints=constraints) if self._server.current_goal.get_goal_status( ).status == GoalStatus.PREEMPTING: self.arm.detach_object(target_name) self.back_to_init_pose(state=1) self.state = STATE.STOP return if fraction > 0.8: print "Retreating..." self.arm.execute(retreat_path) result = self.check(post_grasp_position, limit) replan_state = False replan_times += 1 if not result: self.state = STATE.GRASP_RETREAT_ERR else: self.state = STATE.PRE_GRASP_ERR # pre_grasp planning failed if result: self.state = STATE.PICK_FINISH self.arm.clear_path_constraints()
def main(): #Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) #Start a node rospy.init_node('moveit_node') #Set up the right gripper right_gripper = robot_gripper.Gripper('right') #Calibrate the gripper (other commands won't work unless you do this first) print('Calibrating...') right_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.5 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 right_arm.set_pose_target(goal_1) #Set the start state for the right arm right_arm.set_start_state_to_current_state() #Plan a path right_plan = right_arm.plan() #Execute the plan raw_input( 'Press <Enter> to move the right arm to goal pose 1 (path constraints are never enforced during this motion): ' ) right_arm.execute(right_plan) #Close the right gripper print('Closing...') right_gripper.close() rospy.sleep(1.0) #Open the right gripper print('Opening...') right_gripper.open() rospy.sleep(1.0) print('Done!') #Second goal pose ----------------------------------------------------- rospy.sleep(2.0) 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.3 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 right_arm.set_pose_target(goal_2) #Set the start state for the right arm right_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 = "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) #Plan a path right_plan = right_arm.plan() #Execute the plan raw_input('Press <Enter> to move the right arm to goal pose 2: ') right_arm.execute(right_plan) #Close the right gripper print('Closing...') right_gripper.close() rospy.sleep(1.0) #Open the right gripper print('Opening...') right_gripper.open() rospy.sleep(1.0) print('Done!') #Third goal pose ----------------------------------------------------- rospy.sleep(2.0) goal_3 = PoseStamped() goal_3.header.frame_id = "base" #x, y, and z position goal_3.pose.position.x = 0.6 goal_3.pose.position.y = -0.1 goal_3.pose.position.z = 0.1 #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 right_arm.set_pose_target(goal_3) #Set the start state for the right arm right_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 = "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) #Plan a path right_plan = right_arm.plan() #Execute the plan raw_input('Press <Enter> to move the right arm to goal pose 3: ') right_arm.execute(right_plan) #Close the right gripper print('Closing...') right_gripper.close() rospy.sleep(1.0) #Open the right gripper print('Opening...') right_gripper.open() 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 main(): """ Main Script """ #=================================================== # Code to add gripper # rospy.init_node('gripper_test') # # Set up the right gripper # right_gripper = robot_gripper.Gripper('right_gripper') # #Calibrate the gripper (other commands won't work unless you do this first) # print('Calibrating...') # right_gripper.calibrate() # rospy.sleep(2.0) # #Close the right gripper to hold publisher # # MIGHT NOT NEED THIS # print('Closing...') # right_gripper.close() # rospy.sleep(1.0) #=================================================== ## TF CODE tfBuffer = tf2_ros.Buffer() tfListener = tf2_ros.TransformListener(tfBuffer) ## TF CODE planner = PathPlanner("right_arm") Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned limb = intera_interface.Limb("right") control = Controller(Kp, Kd, Ki, Kw, limb) ## ## Add the obstacle to the planning scene here ## #Tower X = 0.075 Y = 0.075 Z = 0.0675 Xp = 0.0 Yp = -0.0425 Zp = 0.0225 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "ar_marker_1" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa planner.add_box_obstacle([X,Y,Z], "tower", pose) #-------------------------------walls pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = -2 pose.pose.position.y = 0 pose.pose.position.z = 0 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 planner.add_box_obstacle([1,1,5], "left_side_wall", pose) pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = 0 pose.pose.position.y = -2 pose.pose.position.z = 0 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 planner.add_box_obstacle([1,1,5], "right_side_wall", pose) pose = PoseStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = "base" pose.pose.position.x = 0 pose.pose.position.y = 2 pose.pose.position.z = 0 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 planner.add_box_obstacle([1,1,5], "back_wall", pose) #-------------------------------walls #Table X = .2 Y = .2 Z = .01 Xp = 0 Yp = 0 Zp = 0 Xpa = 0.00 Ypa = 0.00 Zpa = 0.00 Wpa = 1.00 pose = PoseStamped() pose.header.stamp = rospy.Time.now() #TODO: Is this the correct frame name? pose.header.frame_id = "ar_marker_1" pose.pose.position.x = Xp pose.pose.position.y = Yp pose.pose.position.z = Zp pose.pose.orientation.x = Xpa pose.pose.orientation.y = Ypa pose.pose.orientation.z = Zpa pose.pose.orientation.w = Wpa planner.add_box_obstacle([X,Y,Z], "table", pose) orien_const = OrientationConstraint() orien_const.link_name = "right_gripper"; orien_const.header.frame_id = "ar_marker_1"; #orien_const.orientation.y = np.pi/2 #orien_const.orientation.w = 1; orien_const.absolute_x_axis_tolerance = 1; orien_const.absolute_y_axis_tolerance = 1; orien_const.absolute_z_axis_tolerance = 1; orien_const.weight = .5; #for stage 2 # orien_const1 = OrientationConstraint() # orien_const1.link_name = "right_gripper"; # orien_const1.header.frame_id = "ar_marker_1"; # #orien_const.orientation.y = np.pi/2 # #orien_const.orientation.w = 1; # orien_const1.absolute_x_axis_tolerance = 0.5; # orien_const1.absolute_y_axis_tolerance = 0.5; # orien_const1.absolute_z_axis_tolerance = 0.5; # orien_const1.weight = 1.0; #rospy.sleep(1.0) # while not rospy.is_shutdown(): # while not rospy.is_shutdown(): current_x = 0 current_y = 0 current_z = 0 offset = 0.14 i = 0 while(i == 0): try: trans = tfBuffer.lookup_transform('ar_marker_1', 'right_gripper_tip', rospy.Time()) tf_px = trans.transform.translation.x tf_py = trans.transform.translation.y tf_pz = trans.transform.translation.z tf_rx = trans.transform.rotation.x tf_ry = trans.transform.rotation.y tf_rz = trans.transform.rotation.z tf_rw = trans.transform.rotation.w current_x = tf_px + offset current_y = tf_py current_z = tf_pz except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): continue i =1 x = current_x y = current_y z = current_z try: #right_gripper_tip goal_1 = PoseStamped() goal_1.header.frame_id = "ar_marker_1" #x, y, and z position goal_1.pose.position.x = -0.0175 goal_1.pose.position.y = -0.0018 goal_1.pose.position.z = 0.065 #0.0825 #Orientation as a quaternion goal_1.pose.orientation.x = 0 goal_1.pose.orientation.y = 0.707 goal_1.pose.orientation.z = 0 goal_1.pose.orientation.w = 0.707 plan = planner.plan_to_pose(goal_1, list())#[orien_const]) if not planner.execute_plan(plan): raise Exception("Execution failed0") except Exception as e: print e
def liftgripper(self, target_z=0.10): # 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 group = self.group while self.target_location_x == -100: rospy.sleep(0.05) current_pose = group.get_current_pose().pose current_orientation = group.get_current_pose().pose.orientation curr_q = [ current_orientation.x, current_orientation.y, current_orientation.z, current_orientation.w ] position_constraint = PositionConstraint() position_constraint.target_point_offset.x = 0.1 position_constraint.target_point_offset.y = 0.2 position_constraint.target_point_offset.z = 0.3 position_constraint.weight = 0.8 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=curr_q[0], y=curr_q[1], z=curr_q[2], w=curr_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.2 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 = True planning_time = 5 threshold = 0.02 time_limit = 120 start_time = time.time() target_x = current_pose.position.x target_y = current_pose.position.y delta_z = (target_z - current_pose.position.z) / 4 print "Attempting to lift gripper" i = 1 # if (delta_z < threshold): # return True # while (current_pose.position.z < target_z) and ((target_z - current_pose.position.z) > 3*threshold) and ((time.time()-start_time) < time_limit): # status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3], # target_x, target_y, current_pose.position.z + i*delta_z, # allow_replanning, planning_time, thresh=threshold) # rospy.sleep(0.1) # current_pose = group.get_current_pose().pose # i += 1 status = self.go_to_pose_goal(curr_q[0], curr_q[1], curr_q[2], curr_q[3], target_x, target_y, target_z, allow_replanning, planning_time, thresh=threshold) rospy.sleep(0.25) print "Successfully lifted gripper to z: ", current_pose.position.z group.clear_path_constraints() # return ((target_z-current_pose.position.z) < 3*threshold or (current_pose.position.z > target_z)) return True
def main(): """ Main Script """ # Setting up head camera head_camera = CameraController("left_hand_camera") head_camera.resolution = (1280, 800) head_camera.open() print("setting balance") happy = False while not happy: e = head_camera.exposure print("exposue value: " + str(e)) e_val = int(input("new exposure value")) head_camera.exposure = e_val satisfaction = str(raw_input("satified? y/n")) happy = 'y' == satisfaction planner = PathPlanner("right_arm") listener = tf.TransformListener() grip = Gripper('right') grip.calibrate() rospy.sleep(3) grip.open() # creating the table obstacle so that Baxter doesn't hit it table_size = np.array([.5, 1, 10]) table_pose = PoseStamped() table_pose.header.frame_id = "base" thickness = 1 table_pose.pose.position.x = .9 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = -.112 - thickness / 2 table_size = np.array([.5, 1, thickness]) planner.add_box_obstacle(table_size, "table", table_pose) raw_input("gripper close") grip.close() # ###load cup positions found using cup_detection.py ran in virtual environment # start_pos_xy = np.load(POURING_CUP_PATH) # goal_pos_xy = np.load(RECEIVING_CUP_PATH) # start_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET) # goal_pos = np.append(start_pos_xy, OBJECT_HEIGHT - GRABBING_OFFSET) # #### END LOADING CUP POSITIONS camera_subscriber = rospy.Subscriber("cameras/left_hand_camera/image", Image, get_img) Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5]) # Stolen from 106B Students Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned cam_pos= [0.188, -0.012, 0.750] ## ## Add the obstacle to the planning scene here ## # Create a path constraint for the arm 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; horizontal = getQuaternion(np.array([0,1,0]), np.pi) z_rot_pos = getQuaternion(np.array([0,0,1]), np.pi / 2) orig = quatMult(z_rot_pos, horizontal) orig = getQuaternion(np.array([0,1,0]), np.pi / 2) #IN THE VIEW OF THE CAMERA #CORNER1--------->CORNER2 # | | # | | # | | #CORNER3 ------------| width = 0.3 length = 0.6 CORNER1 = np.array([0.799, -0.524, -0.03]) CORNER2 = CORNER1 + np.array([-width, 0, 0]) CORNER3 = CORNER1 + np.array([0, length, 0]) #CREATE THE GRID dir1 = CORNER2 - CORNER1 dir2 = CORNER3 - CORNER1 grid_vals = [] ret_vals = [] for i in range(0, 3): for j in range(0, 4): grid = CORNER1 + i * dir1 / 2 + j * dir2 / 3 grid_vals.append(grid) ret_vals.append(np.array([grid[0], grid[1], OBJECT_HEIGHT])) i = -1 while not rospy.is_shutdown(): for g in grid_vals: i += 1 while not rospy.is_shutdown(): try: goal_1 = PoseStamped() goal_1.header.frame_id = "base" #x, y, and z position goal_1.pose.position.x = g[0] goal_1.pose.position.y = g[1] goal_1.pose.position.z = g[2] y = - g[1] + cam_pos[1] x = g[0] - cam_pos[0] q = orig #Orientation as a quaternion goal_1.pose.orientation.x = q[1][0] goal_1.pose.orientation.y = q[1][1] goal_1.pose.orientation.z = q[1][2] goal_1.pose.orientation.w = q[0] plan = planner.plan_to_pose(goal_1, []) if planner.execute_plan(plan): # raise Exception("Execution failed") rospy.sleep(1) #grip.open() raw_input("open") rospy.sleep(1) # plt.imshow(camera_image) print("yay: " + str(i)) pos = listener.lookupTransform("/reference/right_gripper", "/reference/base", rospy.Time(0))[0] print("move succesfully to " + str(pos)) fname = os.path.join(IMAGE_DIR, "calib_{}.jpg".format(i)) skimage.io.imsave(fname, camera_image) print(e) print("index: ", i) else: break print(np.array(ret_vals)) # save the positions of the gripper so that the homography matrix can be calculated np.save(POINTS_DIR, np.array(ret_vals)) print(np.load(POINTS_DIR)) break
def main(): rospy.init_node('arm_obstacle_demo') wait_for_time() argv = rospy.myargv() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeAttachedObject('tray') # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.5 size_y = 0.01 size_z = 0.05 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.9 pose2.pose.orientation.z = 0.2588 pose2.pose.orientation.w = 0.9659 arm = fetch_api.Arm() def shutdown(): arm.cancel_all_goals() planning_scene.clear() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 5, 'execution_timeout': 30, 'group_name': 'arm', 'num_planning_attempts': 10, 'replan': True, 'replan_attempts': 5, 'tolerance': 0.001 } error = arm.move_to_pose(pose1, **kwargs) if error is not None: arm.cancel_all_goals() rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() rospy.sleep(2) kwargs = { 'allowed_planning_time': 120, 'execution_timeout': 120, 'group_name': 'arm', 'num_planning_attempts': 1, 'replan': True, 'replan_attempts': 10, 'tolerance': 0.01 } oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 kwargs['orientation_constraint'] = oc error = arm.move_to_pose(pose2, **kwargs) if error is not None: arm.cancel_all_goals() rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded')
def build(self, tf_timeout=rospy.Duration(5.0)): """Builds the goal message. To set a pose or joint goal, call set_pose_goal or set_joint_goal before calling build. To add a path orientation constraint, call add_path_orientation_constraint first. Args: tf_timeout: rospy.Duration. How long to wait for a TF message. Only used with pose goals. Returns: moveit_msgs/MoveGroupGoal """ goal = MoveGroupGoal() # Set start state goal.request.start_state = copy.deepcopy(self.start_state) # Set goal constraint if self._pose_goal is not None: self._tf_listener.waitForTransform(self._pose_goal.header.frame_id, self.fixed_frame, rospy.Time.now(), tf_timeout) try: pose_transformed = self._tf_listener.transformPose( self.fixed_frame, self._pose_goal) except (tf.LookupException, tf.ConnectivityException): return None c1 = Constraints() c1.position_constraints.append(PositionConstraint()) c1.position_constraints[0].header.frame_id = self.fixed_frame c1.position_constraints[0].link_name = self.gripper_frame b = BoundingVolume() s = SolidPrimitive() s.dimensions = [self.tolerance * self.tolerance] s.type = s.SPHERE b.primitives.append(s) b.primitive_poses.append(self._pose_goal.pose) c1.position_constraints[0].constraint_region = b c1.position_constraints[0].weight = 1.0 c1.orientation_constraints.append(OrientationConstraint()) c1.orientation_constraints[0].header.frame_id = self.fixed_frame c1.orientation_constraints[ 0].orientation = pose_transformed.pose.orientation c1.orientation_constraints[0].link_name = self.gripper_frame c1.orientation_constraints[ 0].absolute_x_axis_tolerance = self.tolerance c1.orientation_constraints[ 0].absolute_y_axis_tolerance = self.tolerance c1.orientation_constraints[ 0].absolute_z_axis_tolerance = self.tolerance c1.orientation_constraints[0].weight = 1.0 goal.request.goal_constraints.append(c1) elif self._joint_names is not None: c1 = Constraints() for i in range(len(self._joint_names)): c1.joint_constraints.append(JointConstraint()) c1.joint_constraints[i].joint_name = self._joint_names[i] c1.joint_constraints[i].position = self._joint_positions[i] c1.joint_constraints[i].tolerance_above = self.tolerance c1.joint_constraints[i].tolerance_below = self.tolerance c1.joint_constraints[i].weight = 1.0 goal.request.goal_constraints.append(c1) # Set path constraints goal.request.path_constraints.orientation_constraints = self._orientation_contraints # Set trajectory constraints # Set planner ID (name of motion planner to use) goal.request.planner_id = self.planner_id # Set group name goal.request.group_name = self.group_name # Set planning attempts goal.request.num_planning_attempts = self.num_planning_attempts # Set planning time goal.request.allowed_planning_time = self.allowed_planning_time # Set scaling factors goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor # Set planning scene diff goal.planning_options.planning_scene_diff = copy.deepcopy( self.planning_scene_diff) # Set is plan only goal.planning_options.plan_only = self.plan_only # Set look around goal.planning_options.look_around = self.look_around # Set replanning options goal.planning_options.replan = self.replan goal.planning_options.replan_attempts = self.replan_attempts goal.planning_options.replan_delay = self.replan_delay return goal
def main(): """ Main Script """ # Path Planner - right_arm for sawyer planner = PathPlanner("right_arm") ## ## OBSTACLES ## # poses = PoseStamped() # poses.pose.position.x = .5 # poses.pose.position.y = 0 # poses.pose.position.z = 0 # poses.pose.orientation.x = 0 # poses.pose.orientation.y = 0 # poses.pose.orientation.z = 0 # poses.pose.orientation.w = 1 # poses.header.frame_id = "base" # planner.add_box_obstacle(np.array([.4,1.2,.1]), "Howard", poses) # ORIENTATION CONSTRAINT 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 #FULL PATH path = [[.8, .05, -.23, "1"], [.6, -.3, 0.0, "2"], [.6, -.1, .1, "3"]] while not rospy.is_shutdown(): for pos in path: while not rospy.is_shutdown(): try: goal = PoseStamped() goal.header.frame_id = "base" goal.pose.position.x = pos[0] goal.pose.position.y = pos[1] goal.pose.position.z = pos[2] goal.pose.orientation.x = 0.0 goal.pose.orientation.y = -1.0 goal.pose.orientation.z = 0.0 goal.pose.orientation.w = 0.0 plan = planner.plan_to_pose(goal, [orien_const]) raw_input( "Press <Enter> to move the right arm to goal pose " + pos[3] + ":") if not planner.execute_plan(plan): raise Exception("Execution failed") except Exception as e: print e traceback.print_exc() else: break
def print_pointlist(self, point_list): # Get differential of way point list differential = self.get_way_point_differential(point_list) differential_num = [0] differential_num = differential_num + differential eef_rotation_change = [0, 0] for i in range(2, len(differential_num)): eef_rotation_change.append(differential_num[i] - differential_num[i - 1]) pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() #### joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1 constraints.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_4' joint_constraint.position = 150 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) # Orientation constrains # constraints = Constraints() orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = self.group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 2 * pi orientation_constraint.absolute_y_axis_tolerance = 2 * pi orientation_constraint.absolute_z_axis_tolerance = 2 * pi constraints.orientation_constraints.append(orientation_constraint) while len(point_list) > 1: # Move the robot point to first point and find the height initial_plan = self.move_to_initial(point_list[1]) joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot.get_current_state().joint_state.position) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] (roll, pitch, yaw) = euler_from_quaternion([ wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w ]) # [wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \ # quaternion_from_euler(roll, pitch, yaw + eef_rotation_change[point_num]) # print(yaw + eef_rotation_change[point_num]) [wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \ quaternion_from_euler(roll, pitch, yaw) waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan) self.print_list_visualize(point_list) elif success_num == 0: break else: # execute success plan self.msg_prit.data = True self.group.execute(execute_plan) self.print_list_visualize(point_list[:success_num]) break self.msg_prit.data = False # Delete the points what already execute if success_num > 0: del (point_list[0:success_num - 1]) del (eef_rotation_change[0:success_num - 1]) # Add obstacle after printing (need to revise) self.group.set_path_constraints(None) print 'all finish'