def set_path_constraints(self, value): # In order to force planning in joint coordinates we add dummy joint # constrains. empty_joint_constraints = JointConstraint() empty_joint_constraints.joint_name = "r1_joint_grip" empty_joint_constraints.position = 0 empty_joint_constraints.tolerance_above = 1.5 empty_joint_constraints.tolerance_below = 1.5 empty_joint_constraints.weight = 1 value.joint_constraints.append(empty_joint_constraints) MoveGroupCommander.set_path_constraints(self, value)
def move_to(self, pos_x, pos_y, pos_z, orien_x, orien_y, orien_z, orien_w, ik, orien_const=None): #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "right_arm" request.ik_request.ik_link_name = "right_gripper" request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = pos_x request.ik_request.pose_stamped.pose.position.y = pos_y request.ik_request.pose_stamped.pose.position.z = pos_z request.ik_request.pose_stamped.pose.orientation.x = orien_x request.ik_request.pose_stamped.pose.orientation.y = orien_y request.ik_request.pose_stamped.pose.orientation.z = orien_z request.ik_request.pose_stamped.pose.orientation.w = orien_w try: #Send the request to the service response = ik(request) #Print the response HERE print(response) group = MoveGroupCommander("right_arm") if orien_const is not None: constraints = Constraints() constraints.orientation_constraints = orien_const group.set_path_constraints(constraints) # Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) # TRY THIS # Setting just the position without specifying the orientation # group.set_position_target([0.5, 0.5, 0.0]) # Plan IK and execute group.go() except rospy.ServiceException, e: print("Service call failed: %s")
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_test') arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() arm.allow_replanning(True) arm.set_planning_time(5) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_init_orientation = Quaternion() target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0) self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation)) current_pose = arm.get_current_pose(end_effector_link) self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation)) arm.set_pose_target(current_pose) arm.go() rospy.sleep(2) constraints = Constraints() orientation_constraint = OrientationConstraint() constraints.name = 'gripper constraint' orientation_constraint.header = target_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.orientation.x = target_init_orientation[0] orientation_constraint.orientation.y = target_init_orientation[1] orientation_constraint.orientation.z = target_init_orientation[2] orientation_constraint.orientation.w = target_init_orientation[3] orientation_constraint.absolute_x_axis_tolerance = 0.03 orientation_constraint.absolute_y_axis_tolerance = 0.03 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) arm.set_path_constraints(constraints) arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __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 __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 constraint 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('right_arm_zero') # 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('right_arm_zero') # 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 inverse_kinematics(): # Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "right_arm" request.ik_request.ik_link_name = "right_gripper" request.ik_request.attempts = 50 request.ik_request.pose_stamped.header.frame_id = "base" # Create joint constraints #This is joint constraint will need to be set at the group constraints = Constraints() joint_constr = JointConstraint() joint_constr.joint_name = "right_j0" joint_constr.position = TORSO_POSITION joint_constr.tolerance_above = TOLERANCE joint_constr.tolerance_below = TOLERANCE joint_constr.weight = 0.5 constraints.joint_constraints.append(joint_constr) # Get the transformed AR Tag (x,y,z) coordinates # Only care about the x coordinate of AR tag; tells use # how far away wall is # x,y, z tell us the origin of the AR Tag x_coord = board_x # DONT CHANGE y_coord = bounding_points[0] z_coord = bounding_points[1] y_width = bounding_points[2] z_height = bounding_points[3] #Creating Path Planning waypoints = [] z_bais = 0 print( "OMMMMMMMMMMMMMMMMMMMMMMMMMGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGG!!!!!!!!!!!!!!" ) print(bounding_points) for i in range(int(float(z_height) / .03)): target_pose = Pose() target_pose.position.x = float(x_coord - PLANNING_BIAS) if i % 2 == 0: #Starting positions target_pose.position.y = float(y_coord) else: target_pose.position.y = y_coord + float(y_width) #Starting positions target_pose.position.z = float(z_coord + z_bais) target_pose.orientation.y = 1.0 / 2**(1 / 2.0) target_pose.orientation.w = 1.0 / 2**(1 / 2.0) waypoints.append(target_pose) z_bais += .03 #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose = target_pose try: #Send the request to the service response = compute_ik(request) group = MoveGroupCommander("right_arm") group.set_max_velocity_scaling_factor(0.75) #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose = target_pose #Creating a Robot Trajectory for the Path Planning jump_thres = 0.0 eef_step = 0.1 path, fraction = group.compute_cartesian_path([target_pose], eef_step, jump_thres) print("Path fraction: {}".format(fraction)) #Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) #Setting the Joint constraint group.set_path_constraints(constraints) if fraction < 0.5: group.go() else: group.execute(path) if i < int(float(z_height) / 0.03) and i > 0: target2 = target_pose target2.position.z += 0.03 path, fraction = group.compute_cartesian_path([target2], eef_step, jump_thres) group.set_path_constraints(constraints) group.execute(path) except rospy.ServiceException, e: print "Service call failed: %s" % e
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 __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) # 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_tolerance(0.00001) # 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('right') # Plan and execute a trajectory to the goal configuration right_arm.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.4 target_pose.pose.position.y = 0.3 target_pose.pose.position.z = 0.4 target_pose.pose.orientation.x = 0.670820393249937 target_pose.pose.orientation.y = 0.223606797749979 target_pose.pose.orientation.z = 0.223606797749979 target_pose.pose.orientation.w = 0.670820393249937 # Set the start state and target pose, then plan and execute right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go() rospy.sleep(2) # 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 = end_effector_link orientation_constraint.absolute_x_axis_tolerance = 0.00001 orientation_constraint.absolute_y_axis_tolerance = 3.14 orientation_constraint.absolute_z_axis_tolerance = 0.00001 orientation_constraint.weight = 1.0 orientation_constraint.orientation = start_pose.pose.orientation # 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 = deepcopy(start_pose) target_pose.header.frame_id = REFERENCE_FRAME target_pose.pose.position.x = 0.4 target_pose.pose.position.y = -0.3 target_pose.pose.position.z = 0.4 target_pose.pose.orientation.x = 0.670820393249937 target_pose.pose.orientation.y = -0.223606797749979 target_pose.pose.orientation.z = -0.223606797749979 target_pose.pose.orientation.w = 0.670820393249937 #target_pose.pose.orientation.x = 0.707107 #target_pose.pose.orientation.y = 0 #target_pose.pose.orientation.z = 0 #target_pose.pose.orientation.w = 0.707107 # 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() # Return to the "resting" configuration stored in the SRDF file right_arm.set_named_target('up') # 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 getOrientation(self, xyz): pass
class MoveCup(): def __init__(self): #basic initiatioon moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_tutorial') self.robot = moveit_commander.RobotCommander() ################ Collision Object self.scene = moveit_commander.PlanningSceneInterface() table = CollisionObject() primitive = SolidPrimitive() primitive.type = primitive.BOX box_pose = geometry_msgs.msg.PoseStamped() box_pose.header.stamp = rospy.Time.now() box_pose.header.frame_id = 'tablelol' box_pose.pose.position.x = 1.25 box_pose.pose.position.y = 0.0 box_pose.pose.position.z = -0.6 table.primitives.append(primitive) table.primitive_poses.append(box_pose) table.operation = table.ADD self.scene.add_box('table', box_pose, size=(.077, .073, .122)) #use joint_group parameter to change which arm it uses? self.joint_group = rospy.get_param('~arm', default="left_arm") self.group = MoveGroupCommander(self.joint_group) #self.group.set_planner_id("BKPIECEkConfigDefault") #this node will scale any tf pose requests to be at most max_reach from the base frame self.max_reach = rospy.get_param('~max_reach', default=1.1) #define a start pose that we can move to before stuff runs self.start_pose = PoseStamped() self.start_pose = self.get_start_pose() #remove this when working for realz self.display_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=10) self.rate = rospy.Rate(1) self.ik_srv = rospy.ServiceProxy( 'ExternalTools/left/PositionKinematicsNode/IKService', SolvePositionIK) self.limb = baxter_interface.Limb('left') self.rangesub = rospy.Subscriber('cup_center', geometry_msgs.msg.Point, self.rangecb) self.stop = False self.planning = False def rangecb(self, point): if self.planning and point.z == 0: rospy.loginfo('stop') self.stop = True self.move_start() self.rangesub.unregister() def callback(self, targetarray): #callback that moves in a constrained path to anything published to /target_poses ##First, scale the position to be withing self.max_reach #new_target = self.project_point(targetarray.data) # rospy.loginfo(self.stop) if not self.stop: self.planning = True new_target = self.project_point(targetarray) target = PoseStamped() target.header.stamp = rospy.Time.now() target.header.frame_id = 'base' target.pose.position = new_target #change orientation to be upright target.pose.orientation = self.start_pose.pose.orientation #clear group info and set it again self.group.clear_pose_targets() # self.group.set_path_constraints(self.get_constraint()) self.group.set_planning_time(10) # self.group.set_pose_target(target) ################### Try joint space planning jt_state = JointState() jt_state.header.stamp = rospy.Time.now() angles = self.limb.joint_angles() jt_state.name = list(angles.keys()) jt_state.position = list(angles.values()) jt_state.header.frame_id = 'base' result = self.ik_srv([target], [jt_state], 0) angles = {} i = 0 for name in result.joints[0].name: angles[name] = result.joints[0].position[i] i = i + 1 self.group.set_joint_value_target(angles) #plan and execute plan. If I find a way, I should add error checking her #currently, if the plan fails, it just doesn't move and waits for another pose to be published plan = self.group.plan() self.group.execute(plan) self.rate.sleep() return def scale_movegroup(self, vel=.9, acc=.9): #slows down baxters arm so we stop getting all those velocity limit errors self.group.set_max_velocity_scaling_factor(vel) self.group.set_max_acceleration_scaling_factor(acc) def unscale_movegroup(self): self.group.set_max_velocity_scaling_factor(1) self.group.set_max_acceleration_scaling_factor(1) def start_baxter_interface(self): #I copied this from an example but have no idea what it does self._pub_rate = rospy.Publisher('robot/joint_state_publish_rate', UInt16, queue_size=10) self._left_arm = baxter_interface.limb.Limb("left") self._left_joint_names = self._left_arm.joint_names() print(self._left_arm.endpoint_pose()) self._rs = baxter_interface.RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() # set joint state publishing to 100Hz self._pub_rate.publish(100) return def get_start_pose(self, point=[.9, 0.2, 0], rpy=[0, math.pi / 2, 0]): #define a starting position for the move_start method new_p = PoseStamped() new_p.header.frame_id = self.robot.get_planning_frame() new_p.pose.position.x = point[0] new_p.pose.position.y = point[1] new_p.pose.position.z = point[2] p_orient = tf.transformations.quaternion_from_euler( rpy[0], rpy[1], rpy[2]) p_orient = Quaternion(*p_orient) new_p.pose.orientation = p_orient return (new_p) def project_point(self, multiarray): #scales an array and returns a point (see: Pose.position) to be within self.max_reach #convert points from sonar ring frame to shoulder frame x = multiarray.data[2] + sr[0] - lls[0] y = multiarray.data[0] + sr[1] - lls[1] z = (-1 * multiarray.data[1]) + sr[2] - lls[2] #scale point to a finite reach distance from the shoulder obj_dist = math.sqrt(x**2 + y**2 + z**2) scale_val = min(self.max_reach / obj_dist, .99) point_scaled = Point() #scale point and bring into the base frames point_scaled.x = scale_val * x + lls[0] point_scaled.y = scale_val * y + lls[1] point_scaled.z = scale_val * z + lls[2] return (point_scaled) def move_random(self): #moves baxter to a random position. used for testing randstate = PoseStamped() randstate = self.group.get_random_pose() self.group.clear_pose_targets() self.group.set_pose_target(randstate) self.group.set_planning_time(8) self.scale_movegroup() plan = self.group.plan() while len( plan.joint_trajectory.points) == 1 and not rospy.is_shutdown(): print('plan no work') plan = self.group.plan() self.group.execute(plan) self.rate.sleep() return def move_random_constrained(self): #move baxter to a random position with constrained path planning. also for testing self.scale_movegroup() randstate = PoseStamped() randstate = self.group.get_random_pose() self.group.clear_pose_targets() self.group.set_pose_target(randstate) self.group.set_path_constraints(self.get_constraint()) self.group.set_planning_time(100) self.scale_movegroup() constrained_plan = self.group.plan() self.group.execute(constrained_plan) self.unscale_movegroup() rospy.sleep(3) return def move_start(self): #move baxter to the self.start_pose pose self.group.clear_pose_targets() self.group.set_pose_target(self.start_pose) self.group.set_planning_time(10) print('moving to start') plan = self.group.plan() self.group.execute(plan) print('at start') self.rate.sleep() return def get_constraint(self, euler_orientation=[0, math.pi / 2, 0], tol=[3, 3, .5]): #method takes euler-angle inputs, this converts it to a quaternion q_orientation = tf.transformations.quaternion_from_euler( euler_orientation[0], euler_orientation[1], euler_orientation[2]) orientation_msg = Quaternion(q_orientation[0], q_orientation[1], q_orientation[2], q_orientation[3]) #defines a constraint that sets the end-effector so a cup in it's hand will be upright, or straight upside-down constraint = Constraints() constraint.name = 'upright_wrist' upright_orientation = OrientationConstraint() upright_orientation.link_name = self.group.get_end_effector_link() upright_orientation.orientation = orientation_msg upright_orientation.absolute_x_axis_tolerance = tol[0] upright_orientation.absolute_y_axis_tolerance = tol[1] upright_orientation.absolute_z_axis_tolerance = tol[2] upright_orientation.weight = 1.0 upright_orientation.header = self.start_pose.header constraint.orientation_constraints.append(upright_orientation) return (constraint)
# 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 rospy.loginfo("Start Pose Target 2") pose_target_2 = Pose() pose_target_2.position.x = 0.3 pose_target_2.position.y = -0.5 pose_target_2.position.z = 0.15 pose_target_2.orientation.x = 0.0 pose_target_2.orientation.y = -0.707 pose_target_2.orientation.z = 0.0 pose_target_2.orientation.w = 0.707 group.set_planner_id("RRTConnectkConfigDefault")
def main(): roscpp_initialize(sys.argv) rospy.init_node('grasp_ur5', anonymous=True) robot = RobotCommander() scene = PlanningSceneInterface() arm_group = MoveGroupCommander("manipulator") q = quaternion_from_euler(1.5701, 0.0, -1.5701) pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = q[0] pose_target.orientation.y = q[1] pose_target.orientation.z = q[2] pose_target.orientation.w = q[3] pose_target.position.z = 0.23 #0.23 pose_target.position.y = 0.11 #0.11 pose_target.position.x = -0.45 #-0.45 arm_group.set_pose_target(pose_target) plan = arm_group.plan(pose_target) while plan[0]!= True: plan = arm_group.plan(pose_target) if plan[0]: traj = plan[1] arm_group.execute(traj, wait = True) arm_group.stop() arm_group.clear_pose_targets() # rospy.sleep(1) #FAKE PLAN WITHOUT RESTRICTIONS # # state = RobotState() # arm_group.set_start_state(state) pose_target.position.z = 0.77 arm_group.set_pose_target(pose_target) plan_fake = arm_group.plan(pose_target) while plan_fake[0] != True: plan_fake = arm_group.plan(pose_target) pose = arm_group.get_current_pose() constraint = Constraints() constraint.name = "restricao" orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = arm_group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 3.14 orientation_constraint.absolute_y_axis_tolerance = 3.14 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1 constraint.orientation_constraints.append(orientation_constraint) arm_group.set_path_constraints(constraint) # state = RobotState() # arm_group.set_start_state(state) pose_target.position.z = 0.77 # 0.77 # pose_target.position.y = -0.11 # -0.11 # pose_target.position.x = 0.31 # 0.31 arm_group.set_pose_target(pose_target) plan = arm_group.plan(pose_target) while plan[0]!= True: plan = arm_group.plan(pose_target) if plan[0]: traj = plan[1] arm_group.execute(traj, wait = True) arm_group.stop() arm_group.clear_pose_targets()
def inverse_kinematics(): # Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "right_arm" request.ik_request.ik_link_name = "right_gripper" request.ik_request.attempts = 50 request.ik_request.pose_stamped.header.frame_id = "base" # Create joint constraints #This is joint constraint will need to be set at the group constraints = Constraints() joint_constr = JointConstraint() joint_constr.joint_name = "right_j0" joint_constr.position = TORSO_POSITION joint_constr.tolerance_above = TOLERANCE joint_constr.tolerance_below = TOLERANCE joint_constr.weight = 0.5 constraints.joint_constraints.append(joint_constr) # Get the transformed AR Tag (x,y,z) coordinates # Only care about the x coordinate of AR tag; tells use # how far away wall is x_coord = board_x y_coord = board_y z_coord = board_z y_bias = raw_input("Input y coordinate: ") z_bias = raw_input("Input z coordinate: ") y_coord += float(y_bias) z_coord += float(z_bias) #Creating Path Planning waypoints = [] target_pose = Pose() target_pose.position.x = float(x_coord - PLANNING_BIAS) target_pose.position.y = float(y_coord) target_pose.position.z = float(z_coord) target_pose.orientation.y = 1.0/2**(1/2.0) target_pose.orientation.w = 1.0/2**(1/2.0) waypoints.append(target_pose) #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose = target_pose try: #Send the request to the service response = compute_ik(request) group = MoveGroupCommander("right_arm") #Creating a Robot Trajectory for the Path Planning jump_thres = 0.0 eef_step = 0.01 path,fraction = group.compute_cartesian_path(waypoints, eef_step, jump_thres) print("Path fraction: {}".format(fraction)) #Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) #Setting the Joint constraint group.set_path_constraints(constraints) if fraction < 0.5: group.go() else: group.execute(path) except rospy.ServiceException, e: print "Service call failed: %s"%e
class PathPlanner: """ This path planner wraps the planning and actuation functionality provided by MoveIt. All positions are arrays holding x, y, and z coordinates. Orientations are arrays holding x, y, z, and w coordinates (in quaternion form). Attributes: frame_id (str): The frame all coordinates are relative to. workspace (list): The bounds of the planning space (a box). Specified as the minimum x, y, and z coordinates, then the maximum x, y, and z coordinates. group_name (str): The MoveIt group name (for example, 'right_arm'). time_limit (float): The maximum number of seconds MoveIt will plan for. robot: The MoveIt robot commander. scene: The planning scene. group: The MoveIt MoveGroup. scene_publisher: A publisher that updates the planning scene. """ PLANNING_SCENE_TOPIC = '/collision_object' def __init__(self, frame_id, group_name, time_limit=10, workspace=None, register_shutdown=True): if workspace is None: workspace = [-2, -2, -2, 2, 2, 2] if rospy.get_param('verbose'): rospy.loginfo('Move group: {}'.format(group_name)) self.frame_id, self.workspace = frame_id, workspace self.time_limit, self.group_name = time_limit, group_name self.robot = RobotCommander() self.scene = PlanningSceneInterface() self.scene_publisher = rospy.Publisher(self.PLANNING_SCENE_TOPIC, CollisionObject, queue_size=10) self.group = MoveGroupCommander(group_name) if register_shutdown: rospy.on_shutdown(self.shutdown) self.group.set_planning_time(time_limit) self.group.set_workspace(workspace) rospy.sleep(0.5) # Sleep to ensure initialization has finished. if rospy.get_param('verbose'): rospy.loginfo('Initialized path planner.') def shutdown(self): """ Stop the path planner safely. """ self.group.stop() del self.group if rospy.get_param('verbose'): rospy.logwarn('Terminated path planner.') def move_to_pose(self, position, orientation=None): """ Move the end effector to the given pose naively. Arguments: position: The x, y, and z coordinates to move to. orientation: The orientation to take (quaternion, optional). Raises (rospy.ServiceException): Failed to execute movement. """ if orientation is None: self.group.set_position_target(position) else: self.group.set_pose_target(create_pose(self.frame_id, position, orientation)) if rospy.get_param('verbose'): log_pose('Moving to pose.', position, orientation) self.group.go(wait=True) self.group.stop() self.group.clear_pose_targets() def move_to_pose_with_planner(self, position, orientation=None, orientation_constraints=None): """ Move the end effector to the given pose, taking into account constraints and planning scene obstacles. Arguments: position: The x, y, and z coordinates to move to. orientation: The orientation to take (quaternion, optional). orientation_constraints (list): A list of `OrientationConstraint` objects created with `make_orientation_constraint`. Returns (bool): Whether or not the movement executed successfully. """ if orientation_constraints is None: orientation_constraints = [] target = create_pose(self.frame_id, position, orientation) if rospy.get_param('verbose'): log_pose('Moving to pose with planner.', position, orientation) for _ in range(3): try: plan = self.plan_to_pose(target, orientation_constraints) if not self.group.execute(plan, True): raise ValueError('Execution failed.') except ValueError as exc: rospy.logwarn('Failed to perform movement: {}. Retrying.'.format(str(exc))) else: break else: raise ValueError('Failed to move to pose.') def plan_to_pose(self, target, orientation_constraints): """ Plan a movement to a pose from the current state, given constraints. Arguments: target: The destination pose. orientation_constraints (list): The constraints. Returns (moveit_msgs.msg.RobotTrajectory): The path. """ self.group.set_pose_target(target) self.group.set_start_state_to_current_state() constraints = Constraints() constraints.orientation_constraints = orientation_constraints self.group.set_path_constraints(constraints) return self.group.plan() def add_box_obstacle(self, dimensions, name, com_position, com_orientation): """ Add a rectangular prism obstacle to the planning scene. Arguments: dimensions: An array containing the width, length, and height of the box (in the box's body frame, corresponding to x, y, and z). name: A unique name for identifying this obstacle. com_position: The position of the center-of-mass (COM) of this box, relative to the global frame `frame_id`. com_orientation: The orientation of the COM. """ pose = create_pose(self.frame_id, com_position, com_orientation) obj = CollisionObject() obj.id, obj.operation, obj.header = name, CollisionObject.ADD, pose.header box = SolidPrimitive() box.type, box.dimensions = SolidPrimitive.BOX, dimensions obj.primitives, obj.primitive_poses = [box], [pose.pose] self.scene_publisher.publish(obj) if rospy.get_param('verbose'): rospy.loginfo('Added box object "{}" to planning scene: ' '(x={}, y={}, z={}).'.format(name, *dimensions)) def remove_obstacle(self, name): """ Remove a named obstacle from the planning scene. """ obj = CollisionObject() obj.id, obj.operation = name, CollisionObject.REMOVE self.scene_publisher.publish(obj) if rospy.get_param('verbose'): rospy.loginfo('Removed object "{}" from planning scene.'.format(name)) def make_orientation_constraint(self, orientation, link_id, tolerance=0.1, weight=1): """ Make an orientation constraint in the context of the robot and world. Arguments: orientation: The orientation the link should have. link_id: The name of the link frame. Returns (OrientationConstraint): The constraint. """ constraint = OrientationConstraint() constraint.header.frame_id = self.frame_id constraint.link_name = link_id const_orien = constraint.orientation const_orien.x, const_orien.y, const_orien.z, const_orien.w = orientation constraint.absolute_x_axis_tolerance = tolerance constraint.absolute_y_axis_tolerance = tolerance constraint.absolute_z_axis_tolerance = tolerance constraint.weight = weight return constraint
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') scene = PlanningSceneInterface('world') self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) self.colors = dict() max_pick_attempts = 10 max_place_attempts = 10 rospy.sleep(1) arm = MoveGroupCommander('arm') end_effector_link = arm.get_end_effector_link() # arm.set_goal_position_tolerance(0.02) # arm.set_goal_orientation_tolerance(0.03) arm.allow_replanning(True) arm.set_planning_time(5) table_id = 'table' side_id = 'side' table2_id = 'table2' box1_id = 'box1' box2_id = 'box2' target_id = 'target' wall_id = 'wall' ground_id = 'ground' scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(table_id) scene.remove_world_object(table2_id) scene.remove_world_object(target_id) scene.remove_world_object(wall_id) scene.remove_world_object(ground_id) scene.remove_world_object(side_id) rospy.sleep(1) table_ground = 0.55 table_size = [0.2, 0.7, 0.01] table2_size = [0.25, 0.6, 0.01] box1_size = [0.02, 0.9, 0.6] box2_size = [0.05, 0.05, 0.15] target_size = [0.03, 0.06, 0.10] wall_size = [0.01, 0.9, 0.6] ground_size = [3, 3, 0.01] side_size = [0.01, 0.7, table_ground] table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME self.setPose(table_pose, [0.7, 0.0, table_ground + table_size[2] / 2.0]) scene.add_box(table_id, table_pose, table_size) side_pose = PoseStamped() side_pose.header.frame_id = REFERENCE_FRAME self.setPose(side_pose, [0.605, 0.0, side_size[2] / 2.0]) scene.add_box(side_id, side_pose, side_size) table2_pose = PoseStamped() table2_pose.header.frame_id = REFERENCE_FRAME self.setPose(table2_pose, [-0.325, 0.0, box1_size[2] + table2_size[2] / 2.0]) # scene.add_box(table2_id, table2_pose, table2_size) box1_pose = PoseStamped() box1_pose.header.frame_id = REFERENCE_FRAME self.setPose(box1_pose, [-0.36, 0.0, box1_size[2] / 2.0]) # scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = REFERENCE_FRAME self.setPose( box2_pose, [0.63, -0.12, table_ground + table_size[2] + box2_size[2] / 2.0]) # scene.add_box(box2_id, box2_pose, box2_size) target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME self.setPose( target_pose, [0.62, 0.0, table_ground + table_size[2] + target_size[2] / 2.0]) scene.add_box(target_id, target_pose, target_size) wall_pose = PoseStamped() wall_pose.header.frame_id = REFERENCE_FRAME self.setPose( wall_pose, [-0.405, 0.0, box1_size[2] + table2_size[2] + wall_size[2] / 2.0]) # scene.add_box(wall_id, wall_pose, wall_size) ground_pose = PoseStamped() ground_pose.header.frame_id = REFERENCE_FRAME self.setPose(ground_pose, [0.0, 0.0, -ground_size[2] / 2.0]) # scene.add_box(ground_id, ground_pose, ground_size) self.setColor(table_id, 0.8, 0.0, 0.0, 1.0) # self.setColor(table2_id, 0.8, 0.0, 0.0) # self.setColor(box1_id, 0.9, 0.9, 0.9) # self.setColor(box2_id, 0.8, 0.4, 1.0) self.setColor(target_id, 0.5, 0.4, 1.0) # self.setColor(wall_id, 0.9, 0.9, 0.9) self.setColor(ground_id, 0.3, 0.3, 0.3, 1.0) self.setColor(side_id, 0.8, 0.0, 0.0, 1.0) self.sendColors() constraints = Constraints() constraints.name = 'gripper constraint' orientation_constraint = OrientationConstraint() arm.set_support_surface_name(table_id) test_pose = PoseStamped() test_pose.header.frame_id = REFERENCE_FRAME test_orientation = Quaternion() test_orientation = quaternion_from_euler(0.0, 1.57, -3.14) self.setPose(test_pose, [ -0.15, -0.22, box1_size[2] + table2_size[2] + target_size[2] / 2.0 ], list(test_orientation)) place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME self.setPose( place_pose, [0.62, -0.22, table_ground + table_size[2] + target_size[2] / 2.0]) grasp_pose = target_pose grasp_init_orientation = Quaternion() grasp_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0) grasp_pose.pose.orientation.x = grasp_init_orientation[0] grasp_pose.pose.orientation.y = grasp_init_orientation[1] grasp_pose.pose.orientation.z = grasp_init_orientation[2] grasp_pose.pose.orientation.w = grasp_init_orientation[3] grasp_pose.pose.position.x -= 0.02 rospy.loginfo('quaterion: ' + str(grasp_pose.pose.orientation)) place_pose.pose.orientation.x = grasp_init_orientation[0] place_pose.pose.orientation.y = grasp_init_orientation[1] place_pose.pose.orientation.z = grasp_init_orientation[2] place_pose.pose.orientation.w = grasp_init_orientation[3] orientation_constraint.header = grasp_pose.header orientation_constraint.link_name = end_effector_link orientation_constraint.orientation.x = grasp_init_orientation[0] orientation_constraint.orientation.y = grasp_init_orientation[1] orientation_constraint.orientation.z = grasp_init_orientation[2] orientation_constraint.orientation.w = grasp_init_orientation[3] orientation_constraint.absolute_x_axis_tolerance = 0.03 orientation_constraint.absolute_y_axis_tolerance = 0.03 orientation_constraint.absolute_z_axis_tolerance = 3.14 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.22, 0.26, [-1.0, 0.0, 0.0]]) result = None n_attempts = 0 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo('Pick attempt:' + str(n_attempts)) rospy.sleep(0.2) arm.set_path_constraints(constraints) arm.set_pose_target(place_pose) arm.go() ''' if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 places = self.make_places(place_pose, [table_id], [0.8, 0.13, [1.0, 0.0, 0.0]], [0.1, 0.12, [-1.0, 0.0, 0.0]]) while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo('Place attempt:' + str(n_attempts)) rospy.sleep(0.2) ''' ''' grasp_pose = place_pose grasp_pose.pose.orientation.x = grasp_init_orientation[0] grasp_pose.pose.orientation.y = grasp_init_orientation[1] grasp_pose.pose.orientation.z = grasp_init_orientation[2] grasp_pose.pose.orientation.w = grasp_init_orientation[3] grasp_pose.pose.position.x -= 0.02 grasps = self.make_grasps(grasp_pose, [table_id], [0.05, 0.07, [1.0, 0.0, 0.0]], [0.15, 0.20, [-1.0, 0.0, 0.0]]) result = None n_attempts = 0 while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo('Pick attempt:' + str(n_attempts)) rospy.sleep(0.2) ''' ''' arm.set_support_surface_name(table2_id) self.setPose(place_pose, [-0.26, -0.22, box1_size[2] + table2_size[2] + target_size[2]/2.0]) if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 places = self.make_places(place_pose, [table2_id], [0.03, 0.06, [-1.0, 0.0, 0.0]], [0.1, 0.15, [1.0, 0.0, 0.0]]) while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo('Place attempt:' + str(n_attempts)) rospy.sleep(0.2) ''' rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class LinkProxy(): def __init__(self, arm): self._arm = arm self._group_name = None self._group = None self._planner = None self.orientation_frozen = False def _initialize_group(self): # get list of planners available to this move group available_planners = rospy.get_param( "/move_group/%s/planner_configs" % self._group_name, None) if available_planners is None: ROS_ERR("The Move group `%s` does not have planners configured.", self._group_name) return False if self._planner not in available_planners: ROS_WARN( "The planner `%s` is not available for the group `%s`, using `%s` instead.", self._planner, self._group_name, available_planners[0]) self._planner = available_planners[0] self._group = MoveGroupCommander(self._group_name) # set planner self._group.set_planner_id(self._planner) def _is_group_valid(self): if self._group is None: if isinstance(self, TerminalLink) or isinstance( self, IntermediateLink): ROS_ERR("Link object was created without a group") return False else: ROS_ERR("Cannot instantiate abstract classes") return False return True def plan_to_pose(self, pose, *args): raise NotImplentedError() def plan_cartesian_path(self, waypoints, *args): raise NotImplentedError() def execute_plan(self, plan): if self._is_group_valid(): return self._arm._execute_plan(self._group, plan) return False def get_pose(self): return self._group.get_current_pose().pose def is_at(self, pose, epsilon): cur_pose = self.get_pose() cur_position = [ cur_pose.position.x, cur_pose.position.y, cur_pose.position.z ] goal_position = [pose.position.x, pose.position.y, pose.position.z] eucl_dist = np.linalg.norm( np.array(cur_position) - np.array(goal_position)) return eucl_dist < epsilon def get_planning_frame(self): if self._is_group_valid(): return self._group.get_planning_frame() return False def freeze_orientation(self): # get current pose cur_pose = self._group.get_current_pose() # create pose constraint constraints = Constraints() constraints.name = "orientation_constraints" orientation_constraint = OrientationConstraint() orientation_constraint.header = cur_pose.header orientation_constraint.header.stamp = rospy.Time.now() orientation_constraint.link_name = self._group.get_end_effector_link() orientation_constraint.orientation = cur_pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 0.5 orientation_constraint.absolute_y_axis_tolerance = 0.5 orientation_constraint.absolute_z_axis_tolerance = 0.5 orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) self._group.set_path_constraints(constraints) self.orientation_frozen = True def clear_path_constraints(self): self._group.set_path_constraints(None) self.orientation_frozen = False