Python moveit_commander.MoveGroupCommander方法代碼示例

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self):
        """
        
        """
        self.ceilheight = 0.75
        rospy.sleep(0.4)
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.sleep(0.4)

        self.scene = moveit_commander.PlanningSceneInterface()
        self.robot = moveit_commander.RobotCommander()

        rospy.sleep(0.1)

        self.group = moveit_commander.MoveGroupCommander("right_arm")
        self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                            moveit_msgs.msg.DisplayTrajectory)
        self.planning_scene_diff_publisher = rospy.Publisher("planning_scene", moveit_msgs.msg.PlanningScene,
                                                             queue_size=1)

        rospy.sleep(0.1)

        self.set_default_planner()

        print "============ Reference frame: %s" % self.group.get_planning_frame()

        print "============ Reference frame: %s" % self.group.get_end_effector_link()
        print self.robot.get_group_names()
        print self.robot.get_current_state()
        self.enable_collision_table1 = True
        self.enable_orientation_constraint = False
        self.set_default_tables_z()
        self.registered_blocks = []

        self.tableshape = (0.913, 0.913, 0.01)
        # self.tableshape = (1.2, 1.2, 0.01)

        rospy.sleep(0.2) 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self, move_group_name, max_velocity_scaling=0.5, max_acceleration_scaling=0.5):
        # self.client = HandeyeClient()  # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic
        self.mgc = MoveGroupCommander(move_group_name)
        self.mgc.set_planner_id("RRTConnectkConfigDefault")
        self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling)
        self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling)
        self.start_pose = self.mgc.get_current_pose()
        self.poses = []
        self.current_pose_index = -1
        self.fallback_joint_limits = [math.radians(90)] * 4 + [math.radians(90)] + [math.radians(180)] + [
            math.radians(350)]
        if len(self.mgc.get_active_joints()) == 6:
            self.fallback_joint_limits = self.fallback_joint_limits[1:] 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def set_group(self, group_name):
        """
        Set the active move group
        :param group_name: move group name
        """
        self.active_group = group_name
        if group_name is None:
            self.active_group = None
            return
        else:
            if group_name not in self.groups:
                if group_name not in self.robot.get_group_names():
                    raise ValueError('Group name %s is not valid. Options are %s' % (group_name, self.robot.get_group_names()))
                self.groups[group_name] = moveit_commander.MoveGroupCommander(group_name)
            self.active_group = self.groups[group_name] 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self):
    self.group = {'left': moveit_commander.MoveGroupCommander("left_arm"),
                  'right': moveit_commander.MoveGroupCommander("right_arm")} 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self):
    self.left_hand = moveit_commander.MoveGroupCommander("left_hand") 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self):
    self.left_arm = moveit_commander.MoveGroupCommander("left_arm")
    self.left_hand = moveit_commander.MoveGroupCommander("left_hand") 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self):
    self.left_arm = moveit_commander.MoveGroupCommander("left_arm") 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def _init_planning_interface(self):
        """ Initializes the MoveIn planning interface """
        self._robot_comm = moveit_commander.RobotCommander()
        self._planning_group = moveit_commander.MoveGroupCommander(self._arm)
        self._planning_group.set_pose_reference_frame(ymc.MOVEIT_PLANNING_REFERENCE_FRAME) 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self):
        # Get params from rosparams
        reference_frame = rospy.get_param("~reference_frame")
        move_group_commander_name = rospy.get_param("~move_group_commander_name")
        allow_replanning = rospy.get_param("~allow_replanning")
        goal_joint_tolerance = rospy.get_param("~goal_joint_tolerance")
        goal_position_tolerance = rospy.get_param("~goal_position_tolerance")
        goal_orientation_tolerance = rospy.get_param("~goal_orientation_tolerance")

        # Set reference_frame
        self.reference_frame = reference_frame

        # Get Arm MoveGroupCommander
        move_group_arm_ok = False
        while not move_group_arm_ok:
            try:
                rospy.loginfo("Trying to get 'arm' group from moveit...")
                self.arm = moveit_commander.MoveGroupCommander(move_group_commander_name)
                move_group_arm_ok = True
            except RuntimeError as e:
                rospy.loginfo(e)
                rospy.sleep(1.0)

        # Get end effector link
        self.end_effector_link = self.arm.get_end_effector_link()

        # Set pose reference frame
        self.arm.set_pose_reference_frame(self.reference_frame)

        # Set planning parameters
        self.arm.allow_replanning(allow_replanning)
        self.arm.set_goal_joint_tolerance(goal_joint_tolerance)
        self.arm.set_goal_position_tolerance(goal_position_tolerance)
        self.arm.set_goal_orientation_tolerance(goal_orientation_tolerance)

        rospy.loginfo("Successfully connected to move_group." +
                      "\n" + "Started group     : " + str(self.arm.get_name()) +
                      "\n" + "Planning_frame    : " + str(self.arm.get_planning_frame()) +
                      "\n" + "Reference frame   : " + str(self.reference_frame) +
                      "\n" + "End effector link : " + str(self.end_effector_link))

        rospy.loginfo("Arm Moveit Commander has been started") 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self,
                 initial_goal,
                 initial_joint_pos,
                 sparse_reward=False,
                 simulated=False,
                 distance_threshold=0.05,
                 target_range=0.15,
                 robot_control_mode='position'):
        """
        Push task for the sawyer robot.

        :param initial_goal: np.array()
                    the initial goal of pnp task,
                    which is object's target position
        :param initial_joint_pos: dict{string: float}
                    initial joint position
        :param sparse_reward: Bool
                    if use sparse reward
        :param simulated: Bool
                    if use simulator
        :param distance_threshold: float
                    threshold for judging if the episode is done
        :param target_range: float
                    the range within which the new target is randomized
        :param robot_control_mode: string
                    control mode 'position'/'velocity'/'effort'
        """
        Serializable.quick_init(self, locals())

        self._distance_threshold = distance_threshold
        self._target_range = target_range
        self._sparse_reward = sparse_reward
        self.initial_goal = initial_goal
        self.goal = self.initial_goal.copy()
        self.simulated = simulated

        # Initialize moveit to get safety check
        self._moveit_robot = moveit_commander.RobotCommander()
        self._moveit_scene = moveit_commander.PlanningSceneInterface()
        self._moveit_group_name = 'right_arm'
        self._moveit_group = moveit_commander.MoveGroupCommander(
            self._moveit_group_name)

        self._robot = Sawyer(
            initial_joint_pos=initial_joint_pos,
            control_mode=robot_control_mode,
            moveit_group=self._moveit_group_name)
        self._world = BlockWorld(self._moveit_scene,
                                 self._moveit_robot.get_planning_frame(),
                                 simulated)

        SawyerEnv.__init__(self, simulated=simulated) 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self,
                 initial_goal,
                 initial_joint_pos,
                 sparse_reward=True,
                 simulated=False,
                 distance_threshold=0.05,
                 target_range=0.15,
                 robot_control_mode='position'):
        """
        Pick-and-place task for the sawyer robot.

        :param initial_goal: np.array()
                    the initial goal of pnp task,
                    which is object's target position
        :param initial_joint_pos: dict{string: float}
                    initial joint position
        :param sparse_reward: Bool
                    if use sparse reward
        :param simulated: Bool
                    if use simulator
        :param distance_threshold: float
                    threshold for judging if the episode is done
        :param target_range: float
                    the range within which the new target is randomized
        :param robot_control_mode: string
                    control mode 'position'/'velocity'/'effort'
        """
        Serializable.quick_init(self, locals())

        self._distance_threshold = distance_threshold
        self._target_range = target_range
        self._sparse_reward = sparse_reward
        self.initial_goal = initial_goal
        self.goal = self.initial_goal.copy()
        self.simulated = simulated

        # Initialize moveit to get safety check
        self._moveit_robot = moveit_commander.RobotCommander()
        self._moveit_scene = moveit_commander.PlanningSceneInterface()
        self._moveit_group_name = 'right_arm'
        self._moveit_group = moveit_commander.MoveGroupCommander(
            self._moveit_group_name)

        self._robot = Sawyer(
            initial_joint_pos=initial_joint_pos,
            control_mode=robot_control_mode,
            moveit_group=self._moveit_group_name)
        self._world = BlockWorld(self._moveit_scene,
                                 self._moveit_robot.get_planning_frame(),
                                 simulated)

        SawyerEnv.__init__(self, simulated=simulated) 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import MoveGroupCommander [as 別名]
def __init__(self,
                 initial_goal,
                 initial_joint_pos,
                 sparse_reward=False,
                 simulated=False,
                 distance_threshold=0.05,
                 target_range=0.15,
                 robot_control_mode='position'):
        """
        Reacher Environment.

        :param initial_goal: np.array
                        the initial goal for the task
        :param initial_joint_pos: dict
                        the initial joint angles for the sawyer
        :param sparse_reward: Bool
                        if use sparse reward
        :param simulated: Bool
                        if run simulated experiment
        :param distance_threshold: float
                        threshold for whether experiment is done
        :param target_range: float
                        delta range the goal is randomized
        :param robot_control_mode: string
                        robot control mode: 'position' or 'velocity'
                        or 'effort'
        """
        Serializable.quick_init(self, locals())

        self._distance_threshold = distance_threshold
        self._target_range = target_range
        self._sparse_reward = sparse_reward
        self.initial_goal = initial_goal
        self.goal = self.initial_goal.copy()
        self.simulated = simulated

        # Initialize moveit to get safety check
        self._moveit_robot = moveit_commander.RobotCommander()
        self._moveit_scene = moveit_commander.PlanningSceneInterface()
        self._moveit_group_name = 'right_arm'
        self._moveit_group = moveit_commander.MoveGroupCommander(
            self._moveit_group_name)

        self._robot = Sawyer(
            initial_joint_pos=initial_joint_pos,
            control_mode=robot_control_mode,
            moveit_group=self._moveit_group_name)
        self._world = EmptyWorld(self._moveit_scene,
                                 self._moveit_robot.get_planning_frame(),
                                 simulated)

        SawyerEnv.__init__(self, simulated=simulated)