def configure(self):
        print 'configure motionplanner'
        # robots
        # init_ctr = [-3.5, -1.]
        init_ctr = [-1.0, 0.0]
        terminal_ctr = [1.5, 1.]
        self.init_pose = [init_ctr[0], init_ctr[1], np.pi/2.]
        self.terminal_pose = [terminal_ctr[0], terminal_ctr[1], 0.0]
        # self._vehicle = omg.Dubins(shapes=omg.Circle(0.35), options={'degree': 2}, bounds={'vmax': 1.5, 'vmin':-0.5,'wmax': np.pi/3., 'wmin': -np.pi/3.})
        # self._vehicle = omg.HolonomicOrient(shapes=omg.Rectangle(0.5, 0.3))
        # self._vehicle.set_options({'safety_distance': 0.1})
        # self._vehicle.set_initial_conditions(self.init_pose)
        # self._vehicle.set_terminal_conditions(self.terminal_pose)
        self._vehicle = omg.Holonomic(shapes=omg.Rectangle(0.5, 0.3))
        self._vehicle.set_options({'safety_distance': 0.1})
        self._vehicle.set_initial_conditions(self.init_pose[:2])
        self._vehicle.set_terminal_conditions(self.terminal_pose[:2])
        # environment
        room = {'shape': omg.Rectangle(4.0, 3.0)}
        self._obstacles = []
        for k, obst in enumerate(self.obs_state.obstacles):
            shape = omg.Circle(obst.radius)
            x = obst.polygon.points[0].x
            y = obst.polygon.points[0].y
            self._obstacles.append(omg.Obstacle({'position': [x , y]}, shape=shape))
        # shape = omg.Beam(width=4.0, height=0.1, orientation=np.pi/2.0)
        # self._obstacles.append(omg.Obstacle({'position': [-2.0, -2.3]}, shape=shape))

        environment = omg.Environment(room=room)
        environment.add_obstacle(self._obstacles)
        # create problem
        print 'creating problem'
        problem = omg.Point2point(self._vehicle, environment, freeT=False)
        problem.set_options({'hard_term_con': False, 'horizon_time': 10.})
        problem.init()
        self._deployer = omg.Deployer(problem, self._sample_time, self._update_time)
        self._deployer.reset()
        self.goal = [0.0, 0.0, 0.0]
예제 #2
0
    def configure(self):
        print 'configure controller'
        # robots
        init_ctr = [-3.5, -1.]
        terminal_ctr = [3.5, 1.]
        self.init_pose = [init_ctr[0], init_ctr[1], np.pi / 2.]
        self.terminal_pose = [terminal_ctr[0], terminal_ctr[1], 0.0]
        # init gazebo
        self.init_gazebo(self.init_pose)
        self._vehicle = omg.Dubins(shapes=omg.Circle(0.35),
                                   options={'degree': 2},
                                   bounds={
                                       'vmax': 0.5,
                                       'wmax': np.pi / 3.,
                                       'wmin': -np.pi / 3.
                                   })
        self._vehicle.set_initial_conditions(self.init_pose)
        self._vehicle.set_terminal_conditions(self.terminal_pose)
        # environment
        room = {'shape': omg.Rectangle(10.0, 5.0)}
        self._obstacles = []
        shape = omg.Beam(width=4.0, height=0.1, orientation=np.pi / 2.0)
        self._obstacles.append(
            omg.Obstacle({'position': [-2.0, -2.3]}, shape=shape))

        environment = omg.Environment(room=room)
        environment.add_obstacle(self._obstacles)
        # create problem
        print 'creating problem'
        problem = omg.Point2point(self._vehicle, environment, freeT=False)
        problem.set_options({'hard_term_con': False, 'horizon_time': 10.})
        problem.init()
        self._deployer = omg.Deployer(problem, self._sample_time,
                                      self._update_time)
        self._deployer.reset()
        self.goal = [0.0, 0.0, 0.0]
예제 #3
0
    def configure(self, st):
        print 'configure motionplanner'
        # timing
        self._sample_time = st.sample_time
        self._update_time = st.update_time
        # robots
        self._n_robots = len(st.fleet_config)
        self._vehicles = [
            omg.Dubins(shapes=omg.Circle(0.35),
                       options={'degree': 2},
                       bounds={
                           'vmax': 0.5,
                           'wmax': np.pi / 3.,
                           'wmin': -np.pi / 3.
                       }) for k in range(self._n_robots)
        ]
        for k in range(self._n_robots):
            self._vehicles[k].define_knots(knot_intervals=10)
        self._fleet = omg.Fleet(self._vehicles)
        if self._n_robots == 1:
            self._fleet.set_initial_conditions([[0., 0., 0.]])
            self._fleet.set_terminal_conditions([[0., 0., 0.]])
        else:
            init_pose = [[0., 0., 0.] for k in range(self._n_robots)]
            terminal_pose = [[0., 0., 0.] for k in range(self._n_robots)]
            self._fleet.set_configuration([[c.pose[0], c.pose[1]]
                                           for c in st.fleet_config],
                                          orientation=st.init_pose[0].pose[2])
            self._fleet.set_initial_conditions(init_pose)
            self._fleet.set_terminal_conditions(terminal_pose)
        # environment
        room = {
            'shape': omg.Rectangle(st.room.shape[0], st.room.shape[1]),
            'position': [st.room.position[0], st.room.position[1]]
        }
        self._obstacles = []
        for k, obst in enumerate(st.obstacles):
            if len(obst.shape) == 1:
                shape = omg.Circle(obst.shape[0])
            elif len(obst.shape) == 2:
                shape = omg.Beam(width=obst.shape[0],
                                 height=obst.shape[1],
                                 orientation=obst.pose[2])
            self._obstacles.append(
                omg.Obstacle({'position': [obst.pose[0], obst.pose[1]]},
                             shape=shape))

        environment = omg.Environment(room=room)
        environment.add_obstacle(self._obstacles)
        self._robobst = st.robobst
        # create problem
        print 'creating problem'
        if self._n_robots == 1:
            problem = omg.Point2point(self._fleet, environment, freeT=False)
            # problem.set_options({'solver_options': {'ipopt': {'ipopt.linear_solver': 'ma57', 'ipopt.hessian_approximation': 'limited-memory'}}})
            problem.set_options({'hard_term_con': False, 'horizon_time': 10.})
        else:
            options = {
                'rho': 5.,
                'horizon_time': 35.,
                'hard_term_con': True,
                'init_iter': 5
            }
            problem = omg.FormationPoint2point(self._fleet,
                                               environment,
                                               options=options)
            # problem.set_options({'solver_options': {'ipopt': {'ipopt.linear_solver': 'ma57', 'ipopt.max_iter': 500}}})
        problem.init()
        self._deployer = omg.Deployer(problem, self._sample_time,
                                      self._update_time)
        self._deployer.reset()
        self._mp_feedback_topic.publish(True)
    def configure(self, data):
        """Configures the motionplanner. Creates omgtools Point2point problem
        with room, static and dynamic obstacles.

        Args:
            data :
                static_obstacles
                dyn_obstacles
                difficult_obst
        """
        mp_configured = False
        self._obstacles = []
        self.n_stat_obst = len(data.static_obstacles)
        self.n_dyn_obst = len(data.dyn_obstacles)

        if data.difficult_obst:
            self.omg_update_time = rospy.get_param(
                'controller/omg_update_time_slow', 0.5)
            safety_margin = rospy.get_param(
                'motionplanner/safety_margin_small', 0.1)
            safety_weight = rospy.get_param('motionplanner/safety_weight_slow',
                                            10.)
            drone_radius = rospy.get_param('motionplanner/drone_radius_small',
                                           0.20)
            vmax = rospy.get_param('motionplanner/omg_vmax_low', 0.2)
            amax = rospy.get_param('motionplanner/omg_amax_low', 0.3)
        else:
            self.omg_update_time = rospy.get_param(
                'controller/omg_update_time', 0.5)
            safety_margin = rospy.get_param('motionplanner/safety_margin', 0.2)
            safety_weight = rospy.get_param('motionplanner/safety_weight', 10.)
            drone_radius = rospy.get_param('motionplanner/drone_radius', 0.225)
            vmax = rospy.get_param('motionplanner/omg_vmax', 0.2)
            amax = rospy.get_param('motionplanner/omg_amax', 0.3)

        print("amax = " + str(amax) + "vmax =" + str(vmax))

        if self.n_dyn_obst:
            safety_margin = rospy.get_param(
                'motionplanner/safety_margin_dyn_obst', 0.2)
            safety_weight = rospy.get_param(
                'motionplanner/safety_weight_dyn_obst', 10.)

        self._vehicle = omg.Holonomic3D(shapes=omg.Sphere(drone_radius),
                                        bounds={
                                            'vmax': vmax,
                                            'vmin': -vmax,
                                            'amax': amax,
                                            'amin': -amax
                                        })
        self._vehicle.define_knots(knot_intervals=self.knots)
        self._vehicle.set_options({
            'safety_distance': safety_margin,
            'safety_weight': safety_weight,
            'syslimit': 'norm_2'
        })
        self._vehicle.set_initial_conditions([0., 0., 0.])
        self._vehicle.set_terminal_conditions([0., 0., 0.])

        # Environment.
        room_width = rospy.get_param('motionplanner/room_width', 1.)
        room_depth = rospy.get_param('motionplanner/room_depth', 1.)
        room_height = rospy.get_param('motionplanner/room_height', 1.)
        room_origin_x = 0.
        room_origin_y = 0.
        room_origin_z = room_height / 2

        room = {
            'shape': omg.Cuboid(room_width, room_depth, room_height),
            'position': [room_origin_x, room_origin_y, room_origin_z]
        }

        # Static obstacles.
        for k, obst in enumerate(data.static_obstacles):
            if obst.obst_type.data == "inf cylinder":
                # 2D shape is extended infinitely along z.
                shape = omg.Circle(obst.shape[0])
                position = [obst.pose[0], obst.pose[1]]

            elif obst.obst_type.data == "slalom plate":
                shape = omg.Beam(obst.shape[1] - 0.1, 0.1, np.pi / 2)
                position = [obst.pose[0], obst.pose[1]]

            elif obst.obst_type.data == "hexagon":
                shape = omg.RegularPrisma(obst.shape[0], obst.shape[1], 6)
                position = [obst.pose[0], obst.pose[1], obst.pose[2]]

            elif obst.obst_type.data == "window plate":
                if (k % 4) <= 1:  # Side plates 1 and 2.
                    shape = omg.Beam(obst.shape[1] - 0.1, 0.1, np.pi / 2)
                    position = [obst.pose[0], obst.pose[1]]
                else:  # Upper and lower plates 3 and 4.
                    shape = omg.Plate(shape2d=omg.Rectangle(
                        obst.shape[0], obst.shape[1]),
                                      height=obst.shape[2],
                                      orientation=[0., np.pi / 2, 0.])
                    position = [obst.pose[0], 0., obst.pose[2]]

            elif obst.obst_type.data == "plate":
                shape = omg.Plate(shape2d=omg.Rectangle(
                    obst.shape[0], obst.shape[1]),
                                  height=obst.shape[2],
                                  orientation=[0., np.pi / 2, obst.direction])
                position = [obst.pose[0], obst.pose[1], obst.pose[2]]

            else:
                print highlight_yellow(' Warning: invalid obstacle type ')

            self._obstacles.append(
                omg.Obstacle({'position': position}, shape=shape))

        # Dynamic obstacles.
        for obst in data.dyn_obstacles:
            shape = omg.Circle(obst.shape[0])
            position = [obst.pose[0], obst.pose[1]]
            self._obstacles.append(
                omg.Obstacle({'position': position}, shape=shape))

        # Create the environment as room with obstacles.
        environment = omg.Environment(room=room)
        environment.add_obstacle(self._obstacles)

        # Create problem.
        problem = omg.Point2point(self._vehicle, environment, freeT=False)
        problem.set_options({
            'solver_options': {
                'ipopt': {
                    'ipopt.linear_solver': 'ma57',
                    'ipopt.max_iter': 1000,
                    'ipopt.print_level': 0,
                    'ipopt.tol': 1e-4,
                    'ipopt.warm_start_init_point': 'yes',
                    'ipopt.warm_start_bound_push': 1e-8,
                    'ipopt.warm_start_mult_bound_push': 1e-8,
                    'ipopt.mu_init': 1e-5,
                    'ipopt.hessian_approximation': 'limited-memory'
                }
            }
        })

        if self.n_dyn_obst:
            problem.set_options({
                'hard_term_con': False,
                'horizon_time': self.horizon_time,
                'verbose': 1.
            })
        else:
            problem.set_options({
                'hard_term_con': True,
                'horizon_time': self.horizon_time,
                'verbose': 1.
            })

        problem.init()
        # problem.fullstop = True

        self._deployer = omg.Deployer(problem, self._sample_time,
                                      self.omg_update_time)
        self._deployer.reset()

        mp_configured = True
        print green('----   Motionplanner running   ----')

        return ConfigMotionplannerResponse(mp_configured)