Python moveit_commander.roscpp_initialize方法代码示例

# 需要导入模块: import moveit_commander [as 别名]
# 或者: from moveit_commander import roscpp_initialize [as 别名]
def run_task(*_):
    """Run task function."""
    initial_goal = np.array([0.6, -0.1, 0.40])

    # Initialize moveit_commander
    moveit_commander.roscpp_initialize(sys.argv)

    rospy.init_node('trpo_sim_sawyer_reacher_exp', anonymous=True)

    env = TheanoEnv(
        ReacherEnv(
            initial_goal,
            initial_joint_pos=INITIAL_ROBOT_JOINT_POS,
            simulated=True))

    rospy.on_shutdown(env.shutdown)

    env.initialize()

    policy = GaussianMLPPolicy(env_spec=env.spec, hidden_sizes=(32, 32))

    baseline = LinearFeatureBaseline(env_spec=env.spec)

    algo = TRPO(
        env=env,
        policy=policy,
        baseline=baseline,
        batch_size=4000,
        max_path_length=100,
        n_itr=100,
        discount=0.99,
        step_size=0.01,
        plot=False,
        force_batch_sampler=True,
    )
    algo.train() 

# 需要导入模块: import moveit_commander [as 别名]
# 或者: from moveit_commander import roscpp_initialize [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 roscpp_initialize [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 roscpp_initialize [as 别名]
def __init__(self, position_manager, trajectory_manager):
        self.trajectory_manager = trajectory_manager
        self.pos_manager = position_manager
        moveit_commander.roscpp_initialize(sys.argv)

        # Load all the sub-commanders
        self.move_group_arm = MoveGroupArm()
        self.arm_commander = ArmCommander(self.move_group_arm)
        self.tool_commander = ToolCommander()

        self.stop_trajectory_server = rospy.Service(
            'niryo_one/commander/stop_command', SetBool, self.callback_stop_command)

        self.reset_controller_pub = rospy.Publisher('/niryo_one/steppers_reset_controller',
                                                    Empty, queue_size=1)

        # robot action server
        self.server = actionlib.ActionServer('niryo_one/commander/robot_action',
                                             RobotMoveAction, self.on_goal, self.on_cancel, auto_start=False)
        self.current_goal_handle = None
        self.learning_mode_on = False
        self.joystick_enabled = False
        self.hardware_status = None
        self.is_active_server = rospy.Service(
            'niryo_one/commander/is_active', GetInt, self.callback_is_active)

        self.learning_mode_subscriber = rospy.Subscriber(
            '/niryo_one/learning_mode', Bool, self.callback_learning_mode)
        self.joystick_enabled_subscriber = rospy.Subscriber('/niryo_one/joystick_interface/is_enabled',
                                                            Bool, self.callback_joystick_enabled)
        self.hardware_status_subscriber = rospy.Subscriber(
            '/niryo_one/hardware_status', HardwareStatus, self.callback_hardware_status)

        self.validation = rospy.get_param("/niryo_one/robot_command_validation")
        self.parameters_validation = ParametersValidation(self.validation)

        # arm velocity
        self.max_velocity_scaling_factor = 100
        self.max_velocity_scaling_factor_pub = rospy.Publisher(
            '/niryo_one/max_velocity_scaling_factor', Int32, queue_size=10)
        self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_arm_max_velocity_scaling_factor)
        self.max_velocity_scaling_factor_server = rospy.Service(
            '/niryo_one/commander/set_max_velocity_scaling_factor', SetInt,
            self.callback_set_max_velocity_scaling_factor)