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