def create_acceleration_pc_fixtures(request):
""" Parameterized Acceleration path constraint.
Return:
-------
data: A tuple. Contains path, ss, alim.
pc: A `PathConstraint`.
"""
dof = request.param
if dof == 1: # Scalar
pi = ta.PolynomialPath([1, 2, 3]) # 1 + 2s + 3s^2
ss = np.linspace(0, 1, 3)
alim = (np.r_[-1., 1]).reshape(1, 2) # Scalar case
accel_const = constraint.JointAccelerationConstraint(
alim, constraint.DiscretizationType.Collocation)
data = (pi, ss, alim)
return data, accel_const
if dof == 2:
coeff = [[1., 2, 3], [-2., -3., 4., 5.]]
pi = ta.PolynomialPath(coeff)
ss = np.linspace(0, 0.75, 4)
alim = np.array([[-1., 2], [-2., 2]])
accel_const = constraint.JointAccelerationConstraint(
alim, constraint.DiscretizationType.Collocation)
data = (pi, ss, alim)
return data, accel_const
if dof == 6:
np.random.seed(10)
N = 20
way_pts = np.random.randn(10, 6)
pi = ta.SplineInterpolator(np.linspace(0, 1, 10), way_pts)
ss = np.linspace(0, 1, N + 1)
vlim_ = np.random.rand(6)
alim = np.vstack((-vlim_, vlim_)).T
accel_const = constraint.JointAccelerationConstraint(
alim, constraint.DiscretizationType.Collocation)
data = (pi, ss, alim)
return data, accel_const
if dof == '6d':
np.random.seed(10)
N = 20
way_pts = np.random.randn(10, 6)
pi = ta.SplineInterpolator(np.linspace(0, 1, 10), way_pts)
ss = np.linspace(0, 1, N + 1)
alim_s = np.random.rand(6)
alim = np.vstack((-alim_s, alim_s)).T
accel_const = constraint.JointAccelerationConstraint(
alim_s, constraint.DiscretizationType.Collocation)
data = (pi, ss, alim)
return data, accel_const
def create_velocity_pc_fixtures(request):
"""Parameterized fixture to test Velocity constraint.
Return:
-------
data: A tuple. Contains path, ss, vim.
pc: A `PathConstraint`.
"""
if request.param == 2:
coeff = [[1., 2, 3], [-2., -3., 4., 5.]]
pi = ta.PolynomialPath(coeff)
ss = np.linspace(0, 0.75, 4)
vlim = np.array([[-1., 2], [-2., 2]])
velocity_constraint = constraint.JointVelocityConstraint(vlim)
data = (pi, ss, vlim)
return data, velocity_constraint
if request.param == 6:
np.random.seed(10)
N = 100
way_pts = np.random.randn(10, 6)
pi = ta.SplineInterpolator(np.linspace(0, 1, 10), way_pts)
ss = np.linspace(0, 1, N + 1)
vlim_ = np.random.rand(6) * 10 + 2.
vlim = np.vstack((-vlim_, vlim_)).T
vel_constraint = constraint.JointVelocityConstraint(vlim)
data = (pi, ss, vlim)
return data, vel_constraint
def basic_path(request):
""" Return a generic path.
"""
if request.param == "spline":
np.random.seed(1)
path = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 7))
elif request.param == "poly":
np.random.seed(1)
coeffs = np.random.randn(7, 3) # 7 random quadratic equations
path = toppra.PolynomialPath(coeffs)
yield path
def test_jnt_vel_varying_basic():
# constraint
ss_wpts = np.r_[0, 1, 2]
vlim_wpts = [[[-1, 2], [-1, 2]], [[-2, 3], [-2, 3]], [[-1, 0], [-1, 0]]]
vlim_spl = CubicSpline(ss_wpts, vlim_wpts)
constraint = ta.constraint.JointVelocityConstraintVarying(vlim_spl)
# path
coeff = [[1., 2, 3], [-2., -3., 4., 5.]]
path = ta.PolynomialPath(coeff)
gridpoints = np.linspace(0, 2, 10)
_, _, _, _, _, _, xlimit = constraint.compute_constraint_params(
path, gridpoints, 1.0)
# constraint splines
qs = path(gridpoints, 1)
# test
sd = cvx.Variable()
for i in range(ss_wpts.shape[0]):
vlim = vlim_spl(gridpoints[i])
# 2. compute max sd from the data
constraints = [
qs[i] * sd <= vlim[:, 1], qs[i] * sd >= vlim[:, 0], sd >= 0,
sd <= JVEL_MAXSD
]
try:
prob = cvx.Problem(cvx.Maximize(sd), constraints)
prob.solve(solver=cvx.ECOS, abstol=1e-9)
xmax = sd.value**2
prob = cvx.Problem(cvx.Minimize(sd), constraints)
prob.solve(solver=cvx.ECOS, abstol=1e-9)
xmin = sd.value**2
except cvx.SolverError:
continue # ECOS can't solve this problem.
# 3. they should agree
npt.assert_allclose([xmin, xmax], xlimit[i], atol=SMALL)
# assert non-negativity
assert xlimit[i, 0] >= 0
def connect_backward(self):
vertex_test = self.tree_forward.vertex_list[-1]
#_#_#the nearest vertex in tree_backward to the last vertex in tree_forward
#_#_#if is_BACKWARD_enable == False, tree_backward has only one vertex
nearest_neighbor_index = self.nearest_neighbor_index(
vertex_test.config, BACKWARD)
status = self.TRAPPED
for n_n_i_hat in nearest_neighbor_index:
print "connect_backward from tree_forward.index={0} to tree_backward.index={1}".format(
vertex_test.index, n_n_i_hat)
vertex_near = self.tree_backward.vertex_list[n_n_i_hat]
q_end = vertex_near.config.q
q_beg = vertex_test.config.q
a0, a1 = utils.interpolate_polynomial_1st(q_beg, q_end)
coefficient_ascend = utils.coefficient_set_1st(a0, a1)
#_#_#print "\tconnect_backward : check_dof_limit"
path_instance = toppra.PolynomialPath(coefficient_ascend)
#_#_#print "\tconnect_backward : check_path_collision"
path_in_collision = utils.check_path_collision(
self.robot, path_instance, 100)
if path_in_collision:
#_#_#print "\t!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
del path_instance
continue
status = self.REACHED
coefficient_descend = utils.coefficient_set_1st(a1, a0)
self.connected_coefficient_descend = coefficient_descend
self.tree_forward.last_vertex_index = vertex_test.index
self.tree_backward.last_vertex_index = vertex_near.index
self.plot_edge(path_instance)
del path_instance
return status
status = self.TRAPPED
return status
def extend_backward(self, config_rand):
amount_dof = self.robot.GetActiveDOF()
status = self.TRAPPED
#_#_#the nearest vertex in tree_backward to the vertex_rand
nearest_neighbor_index = self.nearest_neighbor_index(
config_rand, BACKWARD)
for n_n_i_hat in nearest_neighbor_index:
print "extend_backward from index = {0}".format(n_n_i_hat)
vertex_near = self.tree_backward.vertex_list[n_n_i_hat]
q_end = vertex_near.config.q
#_#_# check if config_rand is too far from vertex_near
delta = self.distance(vertex_near.config, config_rand)
if (delta <= self.STEP_SIZE):
q_beg = config_rand.q
status = self.REACHED
else:
q_beg = q_end + self.STEP_SIZE * (config_rand.q -
q_end) / np.sqrt(delta)
#_#_#???
status = self.ADVANCED
config_new = Config(q_beg)
if not (delta <= self.STEP_SIZE):
delta = self.distance(vertex_near.config, config_new)
n_check_grid = int(delta * 20)
#_#_#print "\textend_backward : Check_Configuration_Collision"
config_in_collision = utils.check_configuration_collision(
self.robot, config_new.q)
if config_in_collision:
status = self.TRAPPED
#_#_#print "\t!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
continue
a0, a1 = utils.interpolate_polynomial_1st(q_beg, q_end)
coefficient_ascend = utils.coefficient_set_1st(a0, a1)
path_instance = toppra.PolynomialPath(coefficient_ascend)
#_#_#print "\textend_backward : check_path_collision"
path_in_collision = utils.check_path_collision(
self.robot,
path_instance,
n_check_grid,
direction_input=BACKWARD)
if path_in_collision:
#_#_#print "\t!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
del path_instance
continue
vertex_new = Vertex(config_new)
vertex_new.level = vertex_near.level + 1
coefficient_descend = utils.coefficient_set_1st(a1, a0)
vertex_new.coefficient_descend = coefficient_descend
self.tree_backward.add_vertex(n_n_i_hat, vertex_new)
self.plot_vertex(vertex_new.config.q)
self.plot_edge(path_instance)
status = self.REACHED
del path_instance
print "extend_backward : Successful extension"
return status
status = self.TRAPPED
return status
