FemPosSmoother¶

`src.python_motion_planning.curve_generation.fem_pos_smooth.FemPosSmoother` ¶

Bases: Curve

Class for Fem-pos smoother.

Parameters:

Examples:

Python Console Session

>>> from python_motion_planning.curve_generation import FemPosSmoother
>>>     points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
>>> generator = FemPosSmoother(w_smooth, w_length, w_ref, dx_l, dx_u, dy_l, dy_u)
>>> generator.run(points)

Source code in src\python_motion_planning\curve_generation\fem_pos_smooth.py

Python

class FemPosSmoother(Curve):
	"""
	Class for Fem-pos smoother.

	Parameters:

	Examples:
		>>> from python_motion_planning.curve_generation import FemPosSmoother
		>>>	points = [(0, 0, 0), (10, 10, -90), (20, 5, 60)]
		>>> generator = FemPosSmoother(w_smooth, w_length, w_ref, dx_l, dx_u, dy_l, dy_u)
		>>> generator.run(points)
	"""
	def __init__(self, w_smooth: float, w_length: float, w_ref: float,
		dx_l: float, dx_u: float, dy_l: float, dy_u: float) -> None:
		super().__init__(0.1)
		self.w_smooth = w_smooth
		self.w_length = w_length
		self.w_ref = w_ref
		self.dx_l = dx_l
		self.dx_u = dx_u
		self.dy_l = dy_l
		self.dy_u = dy_u

	def __str__(self) -> str:
		return "Fem-pos Smoother"

	def generation(self, start_pose: tuple, goal_pose: tuple):
		pass

	def run(self, points: list, display: bool = True):
		"""
		Running both generation and animation.

		Parameters:
			points (list[tuple]): path points
		"""
		assert len(points) >= 3, "Number of points should be at least 3."
		if len(points[0]) == 3:
			points = [(ix, iy) for ix, iy, _ in points]
		import matplotlib.pyplot as plt

		n = len(points)
		P = np.zeros((2 * n, 2 * n))

		X = np.eye(2) * self.w_smooth
		Y = np.eye(2) * self.w_length
		Z = np.eye(2) * self.w_ref

		P[0:2, 0:2] = X + Y + Z
		P[0:2, 2:4] = -2 * X - Y
		P[2:4, 2:4] = 5 * X + 2 * Y + Z
		P[2 * n - 2:2 * n, 2 * n - 2:2 * n] =  X + Y + Z
		P[2 * n - 4:2 * n - 2, 2 * n - 4:2 * n - 2] =  5 * X + 2 * Y + Z
		P[2 * n - 4:2 * n - 2, 2 * n - 2:2 * n] =  -2 * X - Y

		for i in range(2, n - 2):
			P[2 * i:2 * i + 2, 2 * i:2 * i + 2] = 6 * X + 2 * Y + Z
		for i in range(2, n - 1):
			P[2 * i - 2:2 * i, 2 * i:2 * i + 2] = -4 * X - Y
		for i in range(2, n):
			P[2 * i - 4:2 * i - 2, 2 * i:2 * i + 2] = X

		A_I = np.eye(2 * n)
		g = np.zeros((2 * n, 1))
		lower = np.zeros((2 * n, 1))
		upper = np.zeros((2 * n, 1))
		for i, p in enumerate(points):
			g[2 * i], g[2 * i + 1] = -2 * self.w_ref * p[0], -2 * self.w_ref * p[1]
			lower[2 * i], lower[2 * i + 1] = p[0] - self.dx_l, p[1] - self.dy_l
			upper[2 * i], upper[2 * i + 1] = p[0] + self.dx_u, p[1] + self.dy_u

		# solve
		solver = osqp.OSQP()
		solver.setup(sparse.csc_matrix(P), g, sparse.csc_matrix(A_I), lower, upper, verbose=False)
		res = solver.solve()
		opt = res.x

		path_x, path_y = [], []
		for i in range(n):
			path_x.append(opt[2 * i])
			path_y.append(opt[2 * i + 1])

		if display:
			plt.figure("curve generation")
			plt.plot(path_x, path_y, linewidth=2, c="#ff0000", marker="o", label="smooth path")
			raw_x, raw_y = [], []
			for i, (x, y) in enumerate(points):
				# label = "bounding box" if i == 0 else None
				# plt.gca().add_patch(
				# 	plt.Rectangle(xy=(x - self.dx_l, y - self.dy_l), width=self.dx_u + self.dx_l,
				# 	height=self.dy_u + self.dy_l, color='red', linestyle="--", fill=False, label=label)
				# )
				raw_x.append(x)
				raw_y.append(y)
			plt.plot(raw_x, raw_y, linewidth=2, c="#1f77b4", marker="x", label="raw path")
			# plt.axis("equal")
			plt.legend()
			plt.title(str(self))

			plt.show()

		return [(ix, iy) for (ix, iy) in zip(path_x, path_y)]

`run(points, display=True)` ¶

Running both generation and animation.