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]
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)