def test_derivative_simple(self):
np.random.seed(1234)
c = np.array([[4, 3, 2, 1]]).T
dc = np.array([[3 * 4, 2 * 3, 2]]).T
ddc = np.array([[2 * 3 * 4, 1 * 2 * 3]]).T
x = np.array([0, 1])
pp = PPoly(c, x)
dpp = PPoly(dc, x)
ddpp = PPoly(ddc, x)
assert_allclose(pp.derivative().c, dpp.c)
assert_allclose(pp.derivative(2).c, ddpp.c)
def test_derivative_simple(self):
np.random.seed(1234)
c = np.array([[4, 3, 2, 1]]).T
dc = np.array([[3*4, 2*3, 2]]).T
ddc = np.array([[2*3*4, 1*2*3]]).T
x = np.array([0, 1])
pp = PPoly(c, x)
dpp = PPoly(dc, x)
ddpp = PPoly(ddc, x)
assert_allclose(pp.derivative().c, dpp.c)
assert_allclose(pp.derivative(2).c, ddpp.c)
def test_multi_shape(self):
c = np.random.rand(6, 2, 1, 2, 3)
x = np.array([0, 0.5, 1])
p = PPoly(c, x)
assert_equal(p.x.shape, x.shape)
assert_equal(p.c.shape, c.shape)
assert_equal(p(0.3).shape, c.shape[2:])
assert_equal(p(np.random.rand(5, 6)).shape, (5, 6) + c.shape[2:])
dp = p.derivative()
assert_equal(dp.c.shape, (5, 2, 1, 2, 3))
ip = p.antiderivative()
assert_equal(ip.c.shape, (7, 2, 1, 2, 3))
def test_multi_shape(self):
c = np.random.rand(6, 2, 1, 2, 3)
x = np.array([0, 0.5, 1])
p = PPoly(c, x)
assert_equal(p.x.shape, x.shape)
assert_equal(p.c.shape, c.shape)
assert_equal(p(0.3).shape, c.shape[2:])
assert_equal(p(np.random.rand(5,6)).shape,
(5,6) + c.shape[2:])
dp = p.derivative()
assert_equal(dp.c.shape, (5, 2, 1, 2, 3))
ip = p.antiderivative()
assert_equal(ip.c.shape, (7, 2, 1, 2, 3))
class PiecewisePolynomialPath(Interpolator):
""" A class that can generate piecewise polynomial paths, including position, speed and acceleration, calling the class scipy.interpolate.PPoly
Parameters
----------
poly_coeff_set : ndarray, shape (degree_poly, amount_segment, amount_dof)
degree_poly : the coefficients of the polynomials, containing the starting and ending angles of robot joints in each line segment
amount_segment: the number of connections between adjacent nodes in the final path generated in RRT algorithm
amount_dof: DOF of robot
s_waypoints: ndarray
Number of the waypoints.
"""
def __init__(self, poly_coeff_set):
self.poly_coeff_set = np.array(poly_coeff_set)
self.shape = np.array(self.poly_coeff_set.shape)
self.amount_segment = self.shape[1]
self.dof = self.shape[2]
self.degree_poly = self.shape[0]
self.s_start = 0
self.s_end = self.amount_segment
self.duration = self.amount_segment
self.s_waypoint = np.arange(0, self.duration + 1, 1)
self.picecwise_poly = PPoly(self.poly_coeff_set, self.s_waypoint)
self.picecwise_poly_s = self.picecwise_poly.derivative()
self.picecwise_poly_ss = self.picecwise_poly_s.derivative()
import ipdb
ipdb.set_trace()
def get_duration(self):
return self.duration
def eval(self, s_sample):
return self.picecwise_poly(s_sample)
def evald(self, s_sample):
return self.picecwise_poly_s(s_sample)
def evaldd(self, s_sample):
return self.picecwise_poly_ss(s_sample)
def test_extrapolate_attr(self):
# [ 1 - x**2 ]
c = np.array([[-1, 0, 1]]).T
x = np.array([0, 1])
for extrapolate in [True, False, None]:
pp = PPoly(c, x, extrapolate=extrapolate)
pp_d = pp.derivative()
pp_i = pp.antiderivative()
if extrapolate is False:
assert_(np.isnan(pp([-0.1, 1.1])).all())
assert_(np.isnan(pp_i([-0.1, 1.1])).all())
assert_(np.isnan(pp_d([-0.1, 1.1])).all())
assert_equal(pp.roots(), [1])
else:
assert_allclose(pp([-0.1, 1.1]), [1 - 0.1**2, 1 - 1.1**2])
assert_(not np.isnan(pp_i([-0.1, 1.1])).any())
assert_(not np.isnan(pp_d([-0.1, 1.1])).any())
assert_allclose(pp.roots(), [1, -1])
def test_extrapolate_attr(self):
# [ 1 - x**2 ]
c = np.array([[-1, 0, 1]]).T
x = np.array([0, 1])
for extrapolate in [True, False, None]:
pp = PPoly(c, x, extrapolate=extrapolate)
pp_d = pp.derivative()
pp_i = pp.antiderivative()
if extrapolate is False:
assert_(np.isnan(pp([-0.1, 1.1])).all())
assert_(np.isnan(pp_i([-0.1, 1.1])).all())
assert_(np.isnan(pp_d([-0.1, 1.1])).all())
assert_equal(pp.roots(), [1])
else:
assert_allclose(pp([-0.1, 1.1]), [1-0.1**2, 1-1.1**2])
assert_(not np.isnan(pp_i([-0.1, 1.1])).any())
assert_(not np.isnan(pp_d([-0.1, 1.1])).any())
assert_allclose(pp.roots(), [1, -1])
class RaveTrajectoryWrapper(Interpolator):
"""An interpolator that wraps OpenRAVE's :class:`GenericTrajectory`.
Only trajectories using quadratic interpolation or cubic interpolation are supported.
The trajectory is represented as a piecewise polynomial. The polynomial could be
quadratic or cubic depending the interpolation method used by the input trajectory object.
Parameters
----------
traj: :class:`openravepy.GenericTrajectory`
An OpenRAVE joint trajectory.
robot: :class:`openravepy.GenericRobot`
An OpenRAVE robot.
"""
def __init__(self, traj, robot):
self.traj = traj #: init
self.spec = traj.GetConfigurationSpecification()
self.dof = robot.GetActiveDOF()
self._interpolation = self.spec.GetGroupFromName('joint').interpolation
assert self._interpolation == 'quadratic' or self._interpolation == "cubic", "This class only handles trajectories with quadratic or cubic interpolation"
self._duration = traj.GetDuration()
all_waypoints = traj.GetWaypoints(0, traj.GetNumWaypoints()).reshape(
traj.GetNumWaypoints(), -1)
valid_wp_indices = [0]
self.ss_waypoints = [0.0]
for i in range(1, traj.GetNumWaypoints()):
dt = self.spec.ExtractDeltaTime(all_waypoints[i])
if dt > 1e-5: # If delta is too small, skip it.
valid_wp_indices.append(i)
self.ss_waypoints.append(self.ss_waypoints[-1] + dt)
self.n_waypoints = len(valid_wp_indices)
self.ss_waypoints = np.array(self.ss_waypoints)
self.s_start = self.ss_waypoints[0]
self.s_end = self.ss_waypoints[-1]
self.waypoints = np.array([
self.spec.ExtractJointValues(all_waypoints[i], robot,
robot.GetActiveDOFIndices())
for i in valid_wp_indices
])
self.waypoints_d = np.array([
self.spec.ExtractJointValues(all_waypoints[i], robot,
robot.GetActiveDOFIndices(), 1)
for i in valid_wp_indices
])
# Degenerate case: there is only one waypoint.
if self.n_waypoints == 1:
pp_coeffs = np.zeros((1, 1, self.dof))
for idof in range(self.dof):
pp_coeffs[0, 0, idof] = self.waypoints[0, idof]
# A constant function
self.ppoly = PPoly(pp_coeffs, [0, 1])
elif self._interpolation == "quadratic":
self.waypoints_dd = []
for i in range(self.n_waypoints - 1):
qdd = ((self.waypoints_d[i + 1] - self.waypoints_d[i]) /
(self.ss_waypoints[i + 1] - self.ss_waypoints[i]))
self.waypoints_dd.append(qdd)
self.waypoints_dd = np.array(self.waypoints_dd)
# Fill the coefficient matrix for scipy.PPoly class
pp_coeffs = np.zeros((3, self.n_waypoints - 1, self.dof))
for idof in range(self.dof):
for iseg in range(self.n_waypoints - 1):
pp_coeffs[:, iseg, idof] = [
self.waypoints_dd[iseg, idof] / 2,
self.waypoints_d[iseg, idof], self.waypoints[iseg,
idof]
]
self.ppoly = PPoly(pp_coeffs, self.ss_waypoints)
elif self._interpolation == "cubic":
self.waypoints_dd = np.array([
self.spec.ExtractJointValues(all_waypoints[i], robot,
robot.GetActiveDOFIndices(), 2)
for i in valid_wp_indices
])
self.waypoints_ddd = []
for i in range(self.n_waypoints - 1):
qddd = ((self.waypoints_dd[i + 1] - self.waypoints_dd[i]) /
(self.ss_waypoints[i + 1] - self.ss_waypoints[i]))
self.waypoints_ddd.append(qddd)
self.waypoints_ddd = np.array(self.waypoints_ddd)
# Fill the coefficient matrix for scipy.PPoly class
pp_coeffs = np.zeros((4, self.n_waypoints - 1, self.dof))
for idof in range(self.dof):
for iseg in range(self.n_waypoints - 1):
pp_coeffs[:, iseg, idof] = [
self.waypoints_ddd[iseg, idof] / 6,
self.waypoints_dd[iseg, idof] / 2,
self.waypoints_d[iseg, idof], self.waypoints[iseg,
idof]
]
self.ppoly = PPoly(pp_coeffs, self.ss_waypoints)
self.ppoly_d = self.ppoly.derivative()
self.ppoly_dd = self.ppoly.derivative(2)
def get_duration(self):
return self._duration
def eval(self, ss_sam):
return self.ppoly(ss_sam)
def evald(self, ss_sam):
return self.ppoly_d(ss_sam)
def evaldd(self, ss_sam):
return self.ppoly_dd(ss_sam)
