def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions=True, path_constraints=None): """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ if path_constraints: if type(path_constraints) is Constraints: constraints_str = conversions.msg_to_string(path_constraints) else: raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints)) (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions, constraints_str) else: (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions) path = RobotTrajectory() path.deserialize(ser_path) return (path, fraction)
def set_pose_targets(self, poses, end_effector_link=""): """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ if len(end_effector_link) > 0 or self.has_end_effector_link(): if not self._g.set_pose_targets([conversions.pose_to_list(p) if type(p) is Pose else p for p in poses], end_effector_link): raise MoveItCommanderException("Unable to set target poses") else: raise MoveItCommanderException("There is no end effector to set poses for")
def place(self, object_name, location=None, plan_only=False): """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided""" result = False if location is None: result = self._g.place(object_name, plan_only) elif type(location) is PoseStamped: old = self.get_pose_reference_frame() self.set_pose_reference_frame(location.header.frame_id) result = self._g.place(object_name, conversions.pose_to_list(location.pose), plan_only) self.set_pose_reference_frame(old) elif type(location) is Pose: result = self._g.place(object_name, conversions.pose_to_list(location), plan_only) elif type(location) is PlaceLocation: result = self._g.place(object_name, conversions.msg_to_string(location), plan_only) else: raise MoveItCommanderException("Parameter location must be a Pose, PoseStamped or PlaceLocation object") return result
def set_pose_target(self, pose, end_effector_link=""): """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:""" """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ if len(end_effector_link) > 0 or self.has_end_effector_link(): ok = False if type(pose) is PoseStamped: old = self.get_pose_reference_frame() self.set_pose_reference_frame(pose.header.frame_id) ok = self._g.set_pose_target(conversions.pose_to_list(pose.pose), end_effector_link) self.set_pose_reference_frame(old) elif type(pose) is Pose: ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link) else: ok = self._g.set_pose_target(pose, end_effector_link) if not ok: raise MoveItCommanderException("Unable to set target pose") else: raise MoveItCommanderException("There is no end effector to set the pose for")
def all_close(goal, actual, tolerance): """ Convenience method for testing if a list of values are within a tolerance of their counterparts in another list @param: goal A list of floats, a Pose or a PoseStamped @param: actual A list of floats, a Pose or a PoseStamped @param: tolerance A float @returns: bool """ all_equal = True if type(goal) is list: for index in range(len(goal)): if abs(actual[index] - goal[index]) > tolerance: return False elif type(goal) is geometry_msgs.msg.PoseStamped: return all_close(goal.pose, actual.pose, tolerance) elif type(goal) is geometry_msgs.msg.Pose: return all_close(pose_to_list(goal), pose_to_list(actual), tolerance) return True
def set_pose_target(self, pose, end_effector_link=""): """ Set the pose of the end-effector, if one is available. The expected input is a Pose message, a PoseStamped message or a list of 6 floats:""" """ [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ if len(end_effector_link) > 0 or self.has_end_effector_link(): ok = False if type(pose) is PoseStamped: old = self.get_pose_reference_frame() self.set_pose_reference_frame(pose.header.frame_id) ok = self._g.set_pose_target( conversions.pose_to_list(pose.pose), end_effector_link) self.set_pose_reference_frame(old) elif type(pose) is Pose: ok = self._g.set_pose_target(conversions.pose_to_list(pose), end_effector_link) else: ok = self._g.set_pose_target(pose, end_effector_link) if not ok: raise MoveItCommanderException("Unable to set target pose") else: raise MoveItCommanderException( "There is no end effector to set the pose for")
def all_close(goal, actual, tolerance): """ Convenience method for checking if a list of values are within tolerance goal = A list of floats, a Pose, or PoseStamped actual = A list of floats, a Pose, or PoseStamped tolerance = A float returns a bool """ all_equal = True if type(goal) is list: for index in range(len(goal)): if abs(actual[index] - goal[index]) > tolerance: return False elif type(goal) is geometry_msgs.msg.PoseStamped: return all_close(goal.pose, actual.pose, tolerance) elif type(goal) is geometry_msgs.msg.Pose: return all_close(pose_to_list(goal), pose_to_list(actual), tolerance) return True
def all_close(self, goal, actual, tolerance = 0.01): """ Convenience method for testing if a list of values are within a tolerance of their counterparts in another list @param: goal: A list of floats, a Pose or a PoseStamped @param: actual: list of floats, a Pose or a PoseStamped @param: tolerance: A float """ if type(goal) is list: for index in [0, 1, 2]: if abs(actual[index] - goal[index]) > tolerance: return False elif type(goal) is geometry_msgs.msg.PoseStamped: return self.all_close(goal.pose, actual.pose, tolerance) elif type(goal) is geometry_msgs.msg.Pose: # print "pose_to_list(goal):"+str(pose_to_list(goal)) # print "pose_to_list(actual):"+str(pose_to_list(actual)) return self.all_close(pose_to_list(goal), pose_to_list(actual), tolerance) return True
def change_end_pose(group, input_pose): end_goal = pose_to_list(group.get_current_pose().pose) for i, ele in enumerate(input_pose): if ele != None: end_goal[i] = ele # print "\nOUT: ", input_pose, end_goal group.set_pose_target(list_to_pose(end_goal)) if group.go(wait=True): # if group.plan(): print "\nSolution found for ", input_pose print "\nGroup moved to ", end_goal else: print "\nNo solution found for ", input_pose
def place(self, object_name, location=None): """Place the named object at a particular location in the environment or somewhere safe in the world if location is not provided""" result = False if location is None: result = self._g.place(object_name) elif type(location) is PoseStamped: old = self.get_pose_reference_frame() self.set_pose_reference_frame(location.header.frame_id) result = self._g.place(object_name, conversions.pose_to_list(location.pose)) self.set_pose_reference_frame(old) elif type(location) is Pose: result = self._g.place(object_name, conversions.pose_to_list(location)) elif type(location) is PlaceLocation: result = self._g.place(object_name, conversions.msg_to_string(location)) else: raise MoveItCommanderException( "Parameter location must be a Pose, PoseStamped or PlaceLocation object" ) return result
def set_pose_targets(self, poses, end_effector_link=""): """ Set the pose of the end-effector, if one is available. The expected input is a list of poses. Each pose can be a Pose message, a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] """ if len(end_effector_link) > 0 or self.has_end_effector_link(): if not self._g.set_pose_targets( [ conversions.pose_to_list(p) if type(p) is Pose else p for p in poses ], end_effector_link, ): raise MoveItCommanderException("Unable to set target poses") else: raise MoveItCommanderException( "There is no end effector to set poses for")
def all_close(self, goal, actual, tolerance): """ Convenience method for testing if a list of values are within a tolerance of their counterparts in another list @param: goal A list of floats, a Pose or a PoseStamped @param: actual A list of floats, a Pose or a PoseStamped @param: tolerance A float @returns: bool """ try: all_equal = True if type(goal) is list: for index in range(len(goal)): if abs(actual[index] - goal[index]) > tolerance: return False elif type(goal) is PoseStamped: return self.all_close(goal.pose, actual.pose, tolerance) elif type(goal) is Pose: return self.all_close(pose_to_list(goal), pose_to_list(actual), tolerance) return True except TypeError: rospy.logerr( "Incompatible types between goal and actual in 'RobotUR.allClose'" )
def change_end_pose(group, input_pose): pose_goal = geometry_msgs.msg.Pose() end_goal = pose_to_list(pose_goal) for i, ele in enumerate(input_pose): if ele != None: end_goal[i] = ele group.set_pose_target(list_to_pose(end_goal)) if group.go(wait=True): print "Solution found for ", input_pose print "Group moved to ", end_goal else: print "No solution found for", input_pose group.stop() group.clear_pose_targets()
def pose_close(goal, actual, position_tol, orientation_tol): """ Convenience method for testing if a list of values are within a position_tolerance of their counterparts in another list @param: goal A list of floats, a Pose or a PoseStamped @param: actual A list of floats, a Pose or a PoseStamped @param: position_tolerance A float @returns: bool """ if type(goal) is list: pos_actual = np.array(actual[0:3]) q_actual = np.array(actual[3:7]) pos_goal = np.array(goal[0:3]) q_goal = np.array(goal[3:7]) # check position diff = np.abs(pos_goal - pos_actual) position_close = np.all(diff < position_tol) # https://math.stackexchange.com/a/90098 dist = 2 * np.inner(q_goal, q_actual)**2 - 1 diff = abs(np.arccos(dist)) orientation_close = np.all(diff < orientation_tol) return position_close, orientation_close elif type(goal) is PoseStamped: return pose_close(goal.pose, actual.pose, position_tol, orientation_tol) elif type(goal) is Pose: return pose_close(pose_to_list(goal), pose_to_list(actual), position_tol, orientation_tol) return True
def pose_to_lists(pose_msg, orientation_type): """ if orientation_type == euler, retrun euler angles in radians """ # print(orientation_type) pose = pose_to_list(pose_msg) position = pose[0:3] quaternion = pose[3:7] if orientation_type == "euler": euler = euler_from_quaternion(quaternion) orientation = euler elif orientation_type == "quaternion": orientation = quaternion else: raise MoveItCommanderException( "Unknown type, accepts type 'euler' or 'quaternion'") return position, orientation
def pose2affine(pose): """Convert pose to affine transform. Example: >>> p = conversions.list_to_pose([1,2,3,0,1,0,0]) >>> pose2affine(p) array([[-1., 0., 0., 1.], [ 0., 1., 0., 2.], [ 0., 0., -1., 3.], [ 0., 0., 0., 1.]]) """ lst = conversions.pose_to_list(pose) pos = np.array(lst[:3]) rot = np.array(lst[3:]) T = np.zeros((4, 4)) T[np.ix_([0, 1, 2], [0, 1, 2])] = q2mat(rot) T[np.ix_([0, 1, 2], [3])] = pos.reshape(3, 1) T[np.ix_([3], [0, 1, 2, 3])] = [0, 0, 0, 1] return T
def move_pose_relative(self, dpose, velocity): if velocity is None: self.moveg.set_max_velocity_scaling_factor(self.velocity) else: self.moveg.set_max_velocity_scaling_factor(velocity) pose = pose_to_list(self.moveg.get_current_pose().pose) t_xform = np.dot(tr.translation_matrix(pose[0:3]), tr.quaternion_matrix(pose[3:])) s_xform = np.dot(tr.translation_matrix(dpose[0:3]), tr.euler_matrix(*dpose[3:])) xform = np.dot(t_xform, s_xform) pose = tr.translation_from_matrix(xform).tolist() + list( tr.euler_from_matrix(xform)) wp = [list_to_pose(pose)] # waypoints self.moveg.set_start_state(self.robot.get_current_state()) (plan, fraction) = self.moveg.compute_cartesian_path(wp, eef_step=0.01, jump_threshold=0.0) if fraction < 1.0: self.last_error_msg = "No motion plan found." return False v = self.velocity if velocity is None else velocity plan = self.moveg.retime_trajectory(self.robot.get_current_state(), plan, v) try: self.moveg.execute(plan, wait=True) self.moveg.stop() except MoveItCommanderException as e: self.last_error_msg = str(e) return False return True
def all_close(goal, actual, tolerance): """ Convenience method for testing if a list of values are within a tolerance of their counterparts in another list @param: goal A list of floats, a Pose or a PoseStamped @param: actual A list of floats, a Pose or a PoseStamped @param: tolerance A float @returns: bool """ all_equal = True if type(goal) is list: for index in range(len(goal)): if abs(actual[index] - goal[index]) > tolerance: return False elif type(goal) is geometry_msgs.msg.PoseStamped: return all_close(goal.pose, actual.pose, tolerance) elif type(goal) is geometry_msgs.msg.Pose: return all_close(pose_to_list(goal), pose_to_list(actual), tolerance) return True class MoveGroupPythonIntefaceTutorial(object): """MoveGroupPythonIntefaceTutorial""" def __init__(self): super(MoveGroupPythonIntefaceTutorial, self).__init__() ## BEGIN_SUB_TUTORIAL setup ## ## First initialize `moveit_commander`_ and a `rospy`_ node: moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move', anonymous=True) ## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to ## the robot: robot = moveit_commander.RobotCommander() ## Instantiate a `PlanningSceneInterface`_ object. This object is an interface ## to the world surrounding the robot: scene = moveit_commander.PlanningSceneInterface() ## Instantiate a `MoveGroupCommander`_ object. This object is an interface ## to one group of joints. In this case the group is the joints in the Panda ## arm so we set ``group_name = panda_arm``. If you are using a different robot, ## you should change this value to the name of your robot arm planning group. ## This interface can be used to plan and execute motions on the Panda: group_name = "manipulator" group = moveit_commander.MoveGroupCommander(group_name) ## We create a `DisplayTrajectory`_ publisher which is used later to publish ## trajectories for RViz to visualize: display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) ## END_SUB_TUTORIAL ## BEGIN_SUB_TUTORIAL basic_info ## ## Getting Basic Information ## ^^^^^^^^^^^^^^^^^^^^^^^^^ # We can get the name of the reference frame for this robot: planning_frame = group.get_planning_frame() print "============ Reference frame: %s" % planning_frame # We can also print the name of the end-effector link for this group: eef_link = group.get_end_effector_link() print "============ End effector: %s" % eef_link # We can get a list of all the groups in the robot: group_names = robot.get_group_names() print "============ Robot Groups:", robot.get_group_names() # Sometimes for debugging it is useful to print the entire state of the # robot: print "============ Printing robot state" print robot.get_current_state() print "" ## END_SUB_TUTORIAL # Misc variables self.box_name = '' self.robot = robot self.scene = scene self.group = group self.display_trajectory_publisher = display_trajectory_publisher self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names def go_to_joint_state(self): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. group = self.group ## BEGIN_SUB_TUTORIAL plan_to_joint_state ## ## Planning to a Joint Goal ## ^^^^^^^^^^^^^^^^^^^^^^^^ ## The Panda's zero configuration is at a `singularity <https://www.quora.com/Robotics-What-is-meant-by-kinematic-singularity>`_ so the first ## thing we want to do is move it to a slightly better configuration. # We can get the joint values from the group and adjust some of the values: joint_goal = group.get_current_joint_values() joint_goal[0] = 0 joint_goal[1] = -pi/4 joint_goal[2] = 0 joint_goal[3] = -pi/2 joint_goal[4] = 0 joint_goal[5] = pi/3 joint_goal[6] = 0 # The go command can be called with joint values, poses, or without any # parameters if you have already set the pose or joint target for the group group.go(joint_goal, wait=True) # Calling ``stop()`` ensures that there is no residual movement group.stop() ## END_SUB_TUTORIAL # For testing: # Note that since this section of code will not be included in the tutorials # we use the class variable rather than the copied state variable current_joints = self.group.get_current_joint_values() return all_close(joint_goal, current_joints, 0.01) def go_to_pose_goal(self): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. group = self.group ## BEGIN_SUB_TUTORIAL plan_to_pose ## ## Planning to a Pose Goal ## ^^^^^^^^^^^^^^^^^^^^^^^ ## We can plan a motion for this group to a desired pose for the ## end-effector: pose_goal = geometry_msgs.msg.Pose() pose_goal.orientation.w = 1.0 pose_goal.position.x = 0.4 pose_goal.position.y = 0.1 pose_goal.position.z = 0.4 group.set_pose_target(pose_goal) ## Now, we call the planner to compute the plan and execute it. plan = group.go(wait=True) # Calling `stop()` ensures that there is no residual movement group.stop() # It is always good to clear your targets after planning with poses. # Note: there is no equivalent function for clear_joint_value_targets() group.clear_pose_targets() ## END_SUB_TUTORIAL # For testing: # Note that since this section of code will not be included in the tutorials # we use the class variable rather than the copied state variable current_pose = self.group.get_current_pose().pose return all_close(pose_goal, current_pose, 0.01) def plan_cartesian_path(self, scale=1): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. group = self.group ## BEGIN_SUB_TUTORIAL plan_cartesian_path ## ## Cartesian Paths ## ^^^^^^^^^^^^^^^ ## You can plan a Cartesian path directly by specifying a list of waypoints ## for the end-effector to go through: ## waypoints = [] wpose = group.get_current_pose().pose wpose.position.z -= scale * 0.1 # First move up (z) wpose.position.y += scale * 0.2 # and sideways (y) waypoints.append(copy.deepcopy(wpose)) wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) waypoints.append(copy.deepcopy(wpose)) wpose.position.y -= scale * 0.1 # Third move sideways (y) waypoints.append(copy.deepcopy(wpose)) # We want the Cartesian path to be interpolated at a resolution of 1 cm # which is why we will specify 0.01 as the eef_step in Cartesian # translation. We will disable the jump threshold by setting it to 0.0 disabling: (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold # Note: We are just planning, not asking move_group to actually move the robot yet: return plan, fraction ## END_SUB_TUTORIAL def display_trajectory(self, plan): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. robot = self.robot display_trajectory_publisher = self.display_trajectory_publisher ## BEGIN_SUB_TUTORIAL display_trajectory ## ## Displaying a Trajectory ## ^^^^^^^^^^^^^^^^^^^^^^^ ## You can ask RViz to visualize a plan (aka trajectory) for you. But the ## group.plan() method does this automatically so this is not that useful ## here (it just displays the same trajectory again): ## ## A `DisplayTrajectory`_ msg has two primary fields, trajectory_start and trajectory. ## We populate the trajectory_start with our current robot state to copy over ## any AttachedCollisionObjects and add our plan to the trajectory. display_trajectory = moveit_msgs.msg.DisplayTrajectory() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) # Publish display_trajectory_publisher.publish(display_trajectory); ## END_SUB_TUTORIAL def execute_plan(self, plan): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. group = self.group ## BEGIN_SUB_TUTORIAL execute_plan ## ## Executing a Plan ## ^^^^^^^^^^^^^^^^ ## Use execute if you would like the robot to follow ## the plan that has already been computed: group.execute(plan, wait=True) ## **Note:** The robot's current joint state must be within some tolerance of the ## first waypoint in the `RobotTrajectory`_ or ``execute()`` will fail ## END_SUB_TUTORIAL def wait_for_state_update(self, box_is_known=False, box_is_attached=False, timeout=4): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. box_name = self.box_name scene = self.scene ## BEGIN_SUB_TUTORIAL wait_for_scene_update ## ## Ensuring Collision Updates Are Receieved ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## If the Python node dies before publishing a collision object update message, the message ## could get lost and the box will not appear. To ensure that the updates are ## made, we wait until we see the changes reflected in the ## ``get_known_object_names()`` and ``get_known_object_names()`` lists. ## For the purpose of this tutorial, we call this function after adding, ## removing, attaching or detaching an object in the planning scene. We then wait ## until the updates have been made or ``timeout`` seconds have passed start = rospy.get_time() seconds = rospy.get_time() while (seconds - start < timeout) and not rospy.is_shutdown(): # Test if the box is in attached objects attached_objects = scene.get_attached_objects([box_name]) is_attached = len(attached_objects.keys()) > 0 # Test if the box is in the scene. # Note that attaching the box will remove it from known_objects is_known = box_name in scene.get_known_object_names() # Test if we are in the expected state if (box_is_attached == is_attached) and (box_is_known == is_known): return True # Sleep so that we give other threads time on the processor rospy.sleep(0.1) seconds = rospy.get_time() # If we exited the while loop without returning then we timed out return False ## END_SUB_TUTORIAL def add_box(self, timeout=4): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. box_name = self.box_name scene = self.scene ## BEGIN_SUB_TUTORIAL add_box ## ## Adding Objects to the Planning Scene ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## First, we will create a box in the planning scene at the location of the left finger: box_pose = geometry_msgs.msg.PoseStamped() box_pose.header.frame_id = "panda_leftfinger" box_pose.pose.orientation.w = 1.0 box_name = "box" scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1)) ## END_SUB_TUTORIAL # Copy local variables back to class variables. In practice, you should use the class # variables directly unless you have a good reason not to. self.box_name=box_name return self.wait_for_state_update(box_is_known=True, timeout=timeout) def attach_box(self, timeout=4): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. box_name = self.box_name robot = self.robot scene = self.scene eef_link = self.eef_link group_names = self.group_names ## BEGIN_SUB_TUTORIAL attach_object ## ## Attaching Objects to the Robot ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## Next, we will attach the box to the Panda wrist. Manipulating objects requires the ## robot be able to touch them without the planning scene reporting the contact as a ## collision. By adding link names to the ``touch_links`` array, we are telling the ## planning scene to ignore collisions between those links and the box. For the Panda ## robot, we set ``grasping_group = 'hand'``. If you are using a different robot, ## you should change this value to the name of your end effector group name. grasping_group = 'hand' touch_links = robot.get_link_names(group=grasping_group) scene.attach_box(eef_link, box_name, touch_links=touch_links) ## END_SUB_TUTORIAL # We wait for the planning scene to update. return self.wait_for_state_update(box_is_attached=True, box_is_known=False, timeout=timeout) def detach_box(self, timeout=4): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. box_name = self.box_name scene = self.scene eef_link = self.eef_link ## BEGIN_SUB_TUTORIAL detach_object ## ## Detaching Objects from the Robot ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## We can also detach and remove the object from the planning scene: scene.remove_attached_object(eef_link, name=box_name) ## END_SUB_TUTORIAL # We wait for the planning scene to update. return self.wait_for_state_update(box_is_known=True, box_is_attached=False, timeout=timeout) def remove_box(self, timeout=4): # Copy class variables to local variables to make the web tutorials more clear. # In practice, you should use the class variables directly unless you have a good # reason not to. box_name = self.box_name scene = self.scene ## BEGIN_SUB_TUTORIAL remove_object ## ## Removing Objects from the Planning Scene ## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ ## We can remove the box from the world. scene.remove_world_object(box_name) ## **Note:** The object must be detached before we can remove it from the world ## END_SUB_TUTORIAL # We wait for the planning scene to update. return self.wait_for_state_update(box_is_attached=False, box_is_known=False, timeout=timeout) def main(): try: print "============ Press `Enter` to begin the tutorial by setting up the moveit_commander (press ctrl-d to exit) ..." raw_input() tutorial = MoveGroupPythonIntefaceTutorial() print "============ Press `Enter` to execute a movement using a joint state goal ..." raw_input() tutorial.go_to_joint_state() print "============ Press `Enter` to execute a movement using a pose goal ..." raw_input() tutorial.go_to_pose_goal() print "============ Press `Enter` to plan and display a Cartesian path ..." raw_input() cartesian_plan, fraction = tutorial.plan_cartesian_path() print "============ Press `Enter` to display a saved trajectory (this will replay the Cartesian path) ..." raw_input() tutorial.display_trajectory(cartesian_plan) print "============ Press `Enter` to execute a saved path ..." raw_input() tutorial.execute_plan(cartesian_plan) print "============ Press `Enter` to add a box to the planning scene ..." raw_input() tutorial.add_box() print "============ Press `Enter` to attach a Box to the Panda robot ..." raw_input() tutorial.attach_box() print "============ Press `Enter` to plan and execute a path with an attached collision object ..." raw_input() cartesian_plan, fraction = tutorial.plan_cartesian_path(scale=-1) tutorial.execute_plan(cartesian_plan) print "============ Press `Enter` to detach the box from the Panda robot ..." raw_input() tutorial.detach_box() print "============ Press `Enter` to remove the box from the planning scene ..." raw_input() tutorial.remove_box() print "============ Python tutorial demo complete!" except rospy.ROSInterruptException: return except KeyboardInterrupt: return if __name__ == '__main__': main()
moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('forward_kinematics', anonymous=True) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_arm = moveit_commander.MoveGroupCommander("arm") display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) planning_frame = group_arm.get_planning_frame() print "============ \nReference frame: %s" % planning_frame eef_link = group_arm.get_end_effector_link() print "End effector: %s" % eef_link group_names = robot.get_group_names() print "Robot Groups:", robot.get_group_names(), "\n============" print "Group_arm pose\n", group_arm.get_current_pose() print "Joint Angles\n", group_arm.get_current_joint_values() # change_joint_angles(group_arm, [1.2, 0.4, -1.57, 1, 0.3]) # change_joint_angles(group_arm, [-3.0357, 0, 0, 0, 0]) # change_joint_angles(group_arm, [3.24426, 0, 0, 0, 0]) change_joint_angles(group_arm, [0, 0, -1.57, -1.57, -2.47]) # change_joint_angles(group_arm, [0, -1.973232422, -1.99, -2.36344086, -2.616666666]) # change_joint_angles(group_arm, [0, 0.57, 0, 0, 0]) #[0.0, -0.2477628272874607, 0.4472224202239656, 0.3047862245403626, 0.07725225351232594, -0.23323341117218666, 0.9201845589721035] print "\nGroup_arm pose\n", group_arm.get_current_pose() print "Joint Angles\n", group_arm.get_current_joint_values() print "\nGroup_arm pose\n", pose_to_list(group_arm.get_current_pose().pose)
def all_close(goal, actual, tolerance): x0, y0, z0, qx0, qy0, qz0, qw0 = pose_to_list(actual) x1, y1, z1, qx1, qy1, qz1, qw1 = pose_to_list(goal) d = dist((x1, y1, z1), (x0, y0, z0)) cos_phi_half = fabs(qx0 * qx1 + qy0 * qy1 + qz0 * qz1 + qw0 * qw1) return d <= tolerance and cos_phi_half >= cos(tolerance / 2.0)