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