Python moveit_commander.RobotCommander方法代碼示例

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import RobotCommander [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 RobotCommander [as 別名]
def __init__(self, group_name=None):
        self.robot = moveit_commander.RobotCommander()
        self.scene = moveit_commander.PlanningSceneInterface()

        self.groups = {}
        self.active_group = None
        self.set_group(group_name)

        self.reset_publisher = rospy.Publisher('/franka_control/error_recovery/goal', ErrorRecoveryActionGoal, queue_size=1) 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import RobotCommander [as 別名]
def get_robot_state_moveit():
    # moveit_commander.roscpp_initialize(sys.argv)
    # robot = moveit_commander.RobotCommander()
    try:
        current_joint_values = np.array(group.get_current_joint_values())
        diff = abs(current_joint_values - home_joint_values)*180/np.pi
        if np.sum(diff<1) == 6:  # if current joint - home position < 1 degree, we think it is at home
            return 1  # robot at home
        else:
            return 2  # robot is moving
    except:
        return 3  # robot state unknow
        rospy.loginfo("Get robot state failed") 

# 需要導入模塊: import moveit_commander [as 別名]
# 或者: from moveit_commander import RobotCommander [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 RobotCommander [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 RobotCommander [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 RobotCommander [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)