def run_cmd(self, cmd): if "waypoints_npz" in self.traj_profile: file_ = np.load( os.path.join(self.db.get_trajectory_data_dir(), self.traj_profile['waypoints_npz'])) path = toppra.SplineInterpolator(file_['t_waypoints'], file_['waypoints']) elif 't_waypoints' in self.traj_profile: path = toppra.SplineInterpolator(self.traj_profile['t_waypoints'], self.traj_profile['waypoints']) else: raise IOError, "Waypoints not found!" q_samples = path.eval(np.arange(0, path.get_duration(), 0.01)) if cmd == "q": exit(42) elif cmd == "r": return True elif cmd == "": for i, q in enumerate(q_samples): self.robot.SetDOFValues(q, range(6)) T_obj = np.dot(self.attach.GetTransform(), self.T_link_model) self.obj.SetTransform(T_obj) if self.env.CheckCollision(self.robot): print "Robot is in collision at waypoint {:d}!".format(i) time.sleep(self.dt) else: try: i = int(cmd) - 1 self.robot.SetDOFValues(path.waypoints[i], range(6)) T_obj = np.dot(self.attach.GetTransform(), self.T_link_model) self.obj.SetTransform(T_obj) except Exception as e: print(e)
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 _estimate_path(self, multiplier, pc_vel, pc_acc): """Estimate path length from reduced v/a limits. This sets up the path only after pc_vel and pc_acc have been established. """ # check for duplicates self.min_pair_dist, self.t_sum = _check_waypts(self.waypts, pc_vel.vlim, pc_acc.alim) if self.min_pair_dist < JNT_DIST_EPS: # issue a warning and try anyway logger.warning( "Duplicates found in input waypoints. This is not recommended," " especially for the beginning and the end of the trajectory. " "Toppra might throw a controllability exception. " "Attempting to optimise trajectory anyway...") # initial x for toppra's path, essentially normalised time on x axis # rescale by given speed limits. # only applies to ParametrizeSpline. self.path_length_limit = 100 * self.t_sum # empirical magic number # t_sum is the minimum time required to visit all given waypoints. # toppra generally needs a smaller number for controllabiility. # It will find that the needed total path length > t_sum in the end. x_max = 1 if multiplier is None else multiplier * self.t_sum x = np.linspace(0, x_max, self.waypts.shape[0]) logger.debug(f"t_sum = {self.t_sum}, t_sum_multiplier = {multiplier}, " f"estimated path length: {x_max}") # specifying natural here doensn't make a difference # toppra only produces clamped cubic splines return ta.SplineInterpolator(x, self.waypts, bc_type="clamped")
def coefficients_functions(): "Coefficients for the equation: A(q) ddot q + dot q B(q) dot q + C(q) = w" def A(q): return np.array([[np.sin(q[0]), 0], [np.cos(q[1]), q[0] + q[1]]]) def B(q): ret = np.zeros((2, 2, 2)) ret[:, :, 0] = [[np.sin(2 * q[0]), 0], [0, q[1]**2]] ret[:, :, 1] = [[1, 2], [3, q[0]]] return ret def C(q): return np.array([q[0] * q[1], 0]) def F(q): ret = np.zeros((4, 2)) ret[:, 0] = [1, 1, 1, 1] ret[:, 1] = [10 * np.sin(q[0]), q[1], q[0] + q[1], 10] return ret def g(q): return np.array([100, 200]) def inv_dyn(q, qd, qdd): return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q) np.random.seed(0) path = toppra.SplineInterpolator([0, 1, 2, 4], np.random.randn(4, 2)) return A, B, C, F, g, path, inv_dyn
def main(): # waypts = [[0], [5], [10]] # path = ta.SplineInterpolator([0, 0.5, 1.0], waypts) waypts = [[0], [1], [10]] path = ta.SplineInterpolator([0, 0.1, 1.0], waypts) # NOTE: When constructing a path, you must "align" the waypoint # properly yourself. For instance, if the waypoints are [0, 1, 10] # like in the above example, the path position should be aligned # like [0, 0.1, 1.0]. If this is not done, the CubicSpline # Interpolator might result undesirable oscillating paths! vlim = np.array([[-3, 3]]) alim = np.array([[-4, 4]]) pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel', gridpoints=np.linspace(0, 1.0, 1001)) jnt_traj = instance.compute_trajectory(0, 0) duration = jnt_traj.duration print("Found optimal trajectory with duration {:f} sec".format(duration)) ts = np.linspace(0, duration, 100) fig, axs = plt.subplots(3, 1, sharex=True) qs = jnt_traj.eval(ts) qds = jnt_traj.evald(ts) qdds = jnt_traj.evaldd(ts) axs[0].plot(ts, qs) axs[1].plot(ts, qds) axs[2].plot(ts, qdds) plt.show()
def test_scalar_zero_motion(Ngrid): """The simple zero motion trajectory Note: Paths with very small displacement, like the one given in this example, are pathological: the optimal path velocities and accelerations tend to be extremely large and have orders of magnitudes difference. Because of this reason, seidel solver wrapper is unlikely to work well and therefore is not tested. """ waypts = [[0], [1e-8], [0]] path = toppra.SplineInterpolator([0, 0.5, 1.0], waypts) # NOTE: When constructing a path, you must "align" the waypoint # properly yourself. For instance, if the waypoints are [0, 1, 10] # like in the above example, the path position should be aligned # like [0, 0.1, 1.0]. If this is not done, the CubicSpline # Interpolator might result undesirable oscillating paths! vlim = np.array([[-10, 10]]) alim = np.array([[-4, 4]]) pc_vel = toppra.constraint.JointVelocityConstraint(vlim) pc_acc = toppra.constraint.JointAccelerationConstraint( alim, discretization_scheme=toppra.constraint.DiscretizationType.Interpolation) instance = toppra.algorithm.TOPPRA( [pc_vel, pc_acc], path, solver_wrapper='hotqpoases', gridpoints=np.linspace(0, 1.0, Ngrid)) jnt_traj = instance.compute_trajectory(0, 0, return_data=True) # Simply assert success assert jnt_traj is not None assert jnt_traj.duration < 9e-4 # less than 1ms
def reparametrise(x, v_min, v_max, a_min, a_max): # stack points as 12d waypoints waypts = x.reshape((x.shape[0], x.shape[1] * x.shape[2])) # initial path path = ta.SplineInterpolator(np.linspace(0, 1, waypts.shape[0]), waypts) dof = waypts.shape[1] # kinematic constraints vlim = np.repeat([[v_min, v_max]], dof, axis=0) alim = np.repeat([[a_min, a_max]], dof, axis=0) pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel') jnt_traj = instance.compute_trajectory(0, 0) duration = jnt_traj.duration print("Found optimal trajectory with duration {:f} sec".format(duration)) ts = np.linspace(0, duration, int(duration * 1 / 0.01)) qs = jnt_traj.eval(ts) qds = jnt_traj.evald(ts) qdds = jnt_traj.evaldd(ts) # time, position, velocity, acceleration return qs, qds, qdds, ts
def pp_fixture(request): """ Velocity & Acceleration Path Constraint. This test case has only two constraints, one velocity constraint and one acceleration constraint. """ dof = 6 np.random.seed(1) # Use the same randomly generated way pts way_pts = np.random.randn(4, dof) * 0.6 N = 200 path = toppra.SplineInterpolator(np.linspace(0, 1, 4), way_pts) ss = np.linspace(0, 1, N + 1) # Velocity Constraint vlim_ = np.random.rand(dof) * 10 + 10 vlim = np.vstack((-vlim_, vlim_)).T pc_vel = constraint.JointVelocityConstraint(vlim) # Acceleration Constraints alim_ = np.random.rand(dof) * 10 + 100 alim = np.vstack((-alim_, alim_)).T pc_acc = constraint.JointAccelerationConstraint(alim) pcs = [pc_vel, pc_acc] yield pcs, path, ss, vlim, alim print("\n [TearDown] Finish PP Fixture")
def basic_init_fixture(request): """ A fixture for testing basic capability of the solver wrapper. This test case has only two constraints, one velocity constraint and one acceleration constraint. """ dof = 6 np.random.seed(1) # Use the same randomly generated way pts way_pts = np.random.randn(4, dof) * 0.6 N = 200 path = toppra.SplineInterpolator(np.linspace(0, 1, 4), way_pts) ss = np.linspace(0, 1, N + 1) # Velocity Constraint vlim_ = np.random.rand(dof) * 10 + 10 vlim = np.vstack((-vlim_, vlim_)).T pc_vel = constraint.JointVelocityConstraint(vlim) # Acceleration Constraints alim_ = np.random.rand(dof) * 10 + 100 alim = np.vstack((-alim_, alim_)).T pc_acc = constraint.JointAccelerationConstraint(alim) # random Second Order Constraint, only use for testing pc_rand = RandomSecondOrderLinearConstraint(dof) pcs = [pc_vel, pc_acc, pc_rand] yield pcs, path, ss, vlim, alim print("\n [TearDown] Finish PP Fixture")
def path_fixture(request): """ A random geometric path. """ seed = request.param np.random.seed(seed) path = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 5)) yield path
def test_scalar_auto_scaling(Ngrid): """Automatic scaling should lead to better results at slower trajectories. """ waypts = [[0], [1e-8], [0]] path = toppra.SplineInterpolator([0, 0.5, 1.0], waypts) # NOTE: When constructing a path, you must "align" the waypoint # properly yourself. For instance, if the waypoints are [0, 1, 10] # like in the above example, the path position should be aligned # like [0, 0.1, 1.0]. If this is not done, the CubicSpline # Interpolator might result undesirable oscillating paths! vlim = np.array([[-10, 10]]) alim = np.array([[-4, 4]]) pc_vel = toppra.constraint.JointVelocityConstraint(vlim) pc_acc = toppra.constraint.JointAccelerationConstraint( alim, discretization_scheme=toppra.constraint.DiscretizationType. Interpolation) instance = toppra.algorithm.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='hotqpoases', gridpoints=np.linspace(0, 1.0, Ngrid), scaling=-1) jnt_traj, aux_traj, data = instance.compute_trajectory(0, 0, return_data=True) # Simply assert success assert jnt_traj is not None assert jnt_traj.duration < 9e-4 # less than 1 ms
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 main(): # Parameters N_samples = 5 SEED = 9 dof = 7 # Random waypoints used to obtain a random geometric path. Here, # we use spline interpolation. np.random.seed(SEED) way_pts = np.random.randn(N_samples, dof) path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts) # Create velocity bounds, then velocity constraint object vlim_ = np.random.rand(dof) * 20 vlim = np.vstack((-vlim_, vlim_)).T # Create acceleration bounds, then acceleration constraint object alim_ = np.random.rand(dof) * 2 alim = np.vstack((-alim_, alim_)).T pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) # Setup a parametrization instance instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel') # Retime the trajectory, only this step is necessary. t0 = time.time() jnt_traj, aux_traj = instance.compute_trajectory(0, 0) print("Parameterization time: {:} secs".format(time.time() - t0)) ts_sample = np.linspace(0, jnt_traj.get_duration(), 100) qs_sample = jnt_traj.eval(ts_sample) qds_sample = jnt_traj.evald(ts_sample) qdds_sample = jnt_traj.evaldd(ts_sample) plt.plot(ts_sample, qdds_sample) plt.xlabel("Time (s)") plt.ylabel("Joint acceleration (rad/s^2)") plt.show() # Compute the feasible sets and the controllable sets for viewing. # Note that these steps are not necessary. _, sd_vec, _ = instance.compute_parameterization(0, 0) X = instance.compute_feasible_sets() K = instance.compute_controllable_sets(0, 0) X = np.sqrt(X) K = np.sqrt(K) plt.plot(X[:, 0], c='green', label="Feasible sets") plt.plot(X[:, 1], c='green') plt.plot(K[:, 0], '--', c='red', label="Controllable sets") plt.plot(K[:, 1], '--', c='red') plt.plot(sd_vec, label="Velocity profile") plt.title("Path-position path-velocity plot") plt.xlabel("Path position") plt.ylabel("Path velocity square") plt.legend() plt.tight_layout() plt.show()
def test_wrong_dimension(self, velocity_pc_data): data, pc = velocity_pc_data path_wrongdim = ta.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10)) with pytest.raises(ValueError) as e_info: pc.compute_constraint_params(path_wrongdim, [0, 0.5, 1], 1.0) assert e_info.value.args[0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( pc.get_dof(), 10 )
def main(): # Parameters N_samples = 5 SEED = 9 dof = 7 # Random waypoints used to obtain a random geometric path. Here, # we use spline interpolation. np.random.seed(SEED) way_pts = np.random.randn(N_samples, dof) path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts) # Create velocity bounds, then velocity constraint object vlim_ = np.random.rand(dof) * 20 vlim = np.vstack((-vlim_, vlim_)).T # Create acceleration bounds, then acceleration constraint object alim_ = np.random.rand(dof) * 2 alim = np.vstack((-alim_, alim_)).T pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) print(path) # Setup a parametrization instance with hot-qpOASES instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=np.linspace(0, 1, 1001), solver_wrapper='hotqpoases') X = instance.compute_feasible_sets() K = instance.compute_controllable_sets(0, 0) _, sd_vec, _ = instance.compute_parameterization(0, 0) X = np.sqrt(X) K = np.sqrt(K) plt.plot(X[:, 0], c='green', label="Feasible sets") plt.plot(X[:, 1], c='green') plt.plot(K[:, 0], '--', c='red', label="Controllable sets") plt.plot(K[:, 1], '--', c='red') plt.plot(sd_vec, label="Velocity profile") plt.title("Path-position path-velocity plot") plt.xlabel("Path position") plt.ylabel("Path velocity square") plt.legend() plt.tight_layout() plt.show() jnt_traj, aux_traj = instance.compute_trajectory(0, 0) ts_sample = np.linspace(0, jnt_traj.get_duration(), 100) qs_sample = jnt_traj.evaldd(ts_sample) plt.plot(ts_sample, qs_sample) plt.xlabel("Time (s)") plt.ylabel("Joint acceleration (rad/s^2)") plt.show()
def main(env=None, verbose=False, savefig=False): # Setup Logging if verbose: logging.basicConfig(level="DEBUG") else: logging.basicConfig(level="WARN") # Setup the robot, the geometric path and the torque bounds print("Loading the environment") if env is None: env = orpy.Environment() else: env.Reset() following.try_load_denso(env) robot = env.GetRobots()[0] np.random.seed(11) waypoints = np.random.randn(5, 6) * 0.4 path = ta.SplineInterpolator(np.linspace(0, 1, 5), waypoints) tau_max = np.r_[30., 50., 20., 20., 10., 10.] robot.SetDOFTorqueLimits(tau_max) # Nominal case print("Computing controllable sets for the ideal case.") cnst = ta.create_rave_torque_path_constraint(robot, 1) pp = ta.algorithm.TOPPRA([cnst], path) Ks = pp.compute_controllable_sets(0, 0) robust_Ks_dict = {} ellipsoid_axes = ([1, 1, 1], [3, 3, 3], [7, 7, 7]) for vs in ellipsoid_axes: print("Computing controllable sets for perturbation ellipsoid={:}". format(vs)) robust_cnst = ta.constraint.RobustCanonicalLinearConstraint(cnst, vs) robust_pp = ta.algorithm.TOPPRA([robust_cnst], path) robust_Ks = robust_pp.compute_controllable_sets(0, 1e-3) robust_Ks_dict[vs[0]] = robust_Ks plt.plot([0, 100], [0, 0], '--', c='black') plt.plot(Ks[:, 1], '--', c='blue', label="ideal case") plt.plot(robust_Ks_dict[1][:, 1], '--', c='green', label="low perturbation level") plt.plot(robust_Ks_dict[3][:, 1], '--', c='orange', label="medium perturbation level") plt.plot(robust_Ks_dict[7][:, 1], '--', c='red', label="high perturbation level") plt.legend() plt.title("Controllable set for different levels of perturbations.") plt.tight_layout() if savefig: plt.savefig("icra18-compare-robust-controllable-sets.pdf") plt.show()
def _generate_movej_trajectory(self, robot_client, rox_robot, q_initial, q_final, speed_perc): JointTrajectoryWaypoint = RRN.GetStructureType( "com.robotraconteur.robotics.trajectory.JointTrajectoryWaypoint", robot_client) JointTrajectory = RRN.GetStructureType( "com.robotraconteur.robotics.trajectory.JointTrajectory", robot_client) dof = len(rox_robot.joint_type) N_samples = math.ceil( np.max(np.abs(q_final - q_initial)) * 0.2 * 180 / np.pi) N_samples = np.max((N_samples, 20)) ss = np.linspace(0, 1, N_samples) ss = np.linspace(0, 1, N_samples) way_pts = np.zeros((N_samples, dof), dtype=np.float64) for i in range(dof): way_pts[:, i] = np.linspace(q_initial[i], q_final[i], N_samples) vlims = rox_robot.joint_vel_limit alims = rox_robot.joint_acc_limit path = ta.SplineInterpolator(ss, way_pts) pc_vel = constraint.JointVelocityConstraint(vlims * 0.95 * (speed_perc / 100.0)) pc_acc = constraint.JointAccelerationConstraint(alims) instance = algo.TOPPRA([pc_vel, pc_acc], path, parametrizer="ParametrizeConstAccel") jnt_traj = instance.compute_trajectory() if jnt_traj is None: return None ts_sample = np.linspace(0, jnt_traj.duration, N_samples) qs_sample = jnt_traj(ts_sample) waypoints = [] for i in range(N_samples): wp = JointTrajectoryWaypoint() wp.joint_position = qs_sample[i, :] wp.time_from_start = ts_sample[i] waypoints.append(wp) traj = JointTrajectory() traj.joint_names = rox_robot.joint_names traj.waypoints = waypoints return traj
def accel_constraint(request): dof = 5 np.random.seed(0) alim_ = np.random.rand(5) alim = np.vstack((-alim_, alim_)).T constraint = toppra.constraint.JointAccelerationConstraint(alim) np.random.seed(0) path = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, dof)) yield constraint, path
def test_wrong_dimension(accel_constraint_setup): _, path_constraint = accel_constraint_setup path_wrongdim = ta.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10)) with pytest.raises(ValueError) as e_info: path_constraint.compute_constraint_params(path_wrongdim, np.r_[0, 0.5, 1], 1.0) assert e_info.value.args[ 0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( path_constraint.get_dof(), 10)
def test_wrong_dimension(coefficients_functions): A, B, C, cnst_F, cnst_g, path = coefficients_functions def inv_dyn(q, qd, qdd): return A(q).dot(qdd) + np.dot(qd.T, np.dot(B(q), qd)) + C(q) constraint = toppra.constraint.CanonicalLinearSecondOrderConstraint(inv_dyn, cnst_F, cnst_g, dof=2) path_wrongdim = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10)) with pytest.raises(AssertionError) as e_info: constraint.compute_constraint_params(path_wrongdim, np.r_[0, 0.5, 1], 1.0) assert e_info.value.args[0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( constraint.get_dof(), 10 )
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 main(): way_pts = generate_new_problem() path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts) pc_vel = constraint.JointVelocityConstraint(10 + np.random.rand(path.dof) * 20) pc_acc = constraint.JointAccelerationConstraint(10 + np.random.rand(path.dof) * 2) # Setup a parametrization instance. The keyword arguments are optional. instance = algo.TOPPRA([pc_vel, pc_acc], path) # Retime the trajectory, only this step is necessary. t0 = time.time() jnt_traj, _, data = instance.compute_trajectory(0, 0, return_data=True) # return_data flag outputs internal data obtained while computing # the paramterization. This include the time stamps corresponding # to the original waypoints. See below (line 53) to see how to # extract the time stamps. print("Parameterization time: {:} secs".format(time.time() - t0)) ts_sample = np.linspace(0, jnt_traj.duration, 100) qs_sample = jnt_traj(ts_sample) for i in range(path.dof): # plot the i-th joint trajectory plt.plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i)) # plot the i-th joint waypoints plt.plot(data['t_waypts'], way_pts[:, i], 'x', c="C{:d}".format(i)) plt.xlabel("Time (s)") plt.ylabel("Joint position (rad/s^2)") plt.show() # Compute the feasible sets and the controllable sets for viewing. # Note that these steps are not necessary. _, sd_vec, _ = instance.compute_parameterization(0, 0) X = instance.compute_feasible_sets() K = instance.compute_controllable_sets(0, 0) X = np.sqrt(X) K = np.sqrt(K) plt.plot(X[:, 0], c='green', label="Feasible sets") plt.plot(X[:, 1], c='green') plt.plot(K[:, 0], '--', c='red', label="Controllable sets") plt.plot(K[:, 1], '--', c='red') plt.plot(sd_vec, label="Velocity profile") plt.title("Path-position path-velocity plot") plt.xlabel("Path position") plt.ylabel("Path velocity square") plt.legend() plt.tight_layout() plt.show()
def test_torque_bound_barret(barret_robot, dof): barret_robot.SetActiveDOFs(range(dof)) constraint = toppra.create_rave_torque_path_constraint(barret_robot) np.random.seed(0) path = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.rand(5, dof)) a, b, c, F, g, _, _ = constraint.compute_constraint_params( path, np.linspace(0, path.get_duration(), 5), 1.0) assert a.shape[1] == dof assert b.shape[1] == dof assert c.shape[1] == dof assert F.shape[1:] == (2 * dof, dof) assert g.shape[1] == 2 * dof
def generateToppraTrajectoryCallback(self, req): print "Generating TOPP-RA trajectory." res = GenerateTrajectoryResponse() dof = len(req.waypoints.points[0].positions) n = len(req.waypoints.points) # If there is not enough waypoints to generate a trajectory return false if (n <= 1 or dof == 0): print "You must provide at least 2 points to generate a valid trajectory." res.trajectory.success = False return res # Generate trajectory. # First set up waypoints. We know hom many will be from n and dof. way_pts = np.zeros([n, dof]) # Next fill the waypoints with data from request. for i in range(0, n): for j in range(0, dof): way_pts[i][j] = req.waypoints.points[i].positions[j] # Part of TOPP-RA is to generate path(s \in [0,1]) from n waypoints. # The algorithm then parametrizes the initial path. path = ta.SplineInterpolator(np.linspace(0, 1, n), way_pts) # Create velocity and acceleration bounds. Supposing symmetrical bounds around zero. vlim_ = np.zeros([dof]) alim_ = np.zeros([dof]) for i in range(0, dof): vlim_[i] = req.waypoints.points[0].velocities[i] alim_[i] = req.waypoints.points[0].accelerations[i] vlim = np.vstack((-vlim_, vlim_)).T alim = np.vstack((-alim_, alim_)).T pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) # Setup a parametrization instance instance = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper='seidel') # Retime the trajectory, only this step is necessary. t0 = time.time() jnt_traj, aux_traj = instance.compute_trajectory(0, 0) #print("Parameterization time: {:} secs".format(time.time() - t0)) # Convert to JointTrajectory message res.trajectory = self.TOPPRA2JointTrajectory(jnt_traj, req.sampling_frequency) res.success = True return res
def test_wrong_dimension(coefficients_functions): """If the given path has wrong dimension, raise error.""" A, B, C, cnst_F, cnst_g, path, inv_dyn = coefficients_functions constraint = toppra.constraint.SecondOrderConstraint(inv_dyn, cnst_F, cnst_g, dof=2) path_wrongdim = toppra.SplineInterpolator(np.linspace(0, 1, 5), np.random.randn(5, 10)) with pytest.raises(ValueError) as e_info: constraint.compute_constraint_params(path_wrongdim, np.r_[0, 0.5, 1]) assert e_info.value.args[ 0] == "Wrong dimension: constraint dof ({:d}) not equal to path dof ({:d})".format( constraint.dof, 10)
def main(): ss, way_pts, vlims, alims = generate_new_problem() path = ta.SplineInterpolator(ss, way_pts) pc_vel = constraint.JointVelocityConstraint(vlims) pc_acc = constraint.JointAccelerationConstraint(alims) # Setup a parametrization instance. The keyword arguments are optional. instance = algo.TOPPRA([pc_vel, pc_acc], path) # Retime the trajectory, only this step is necessary. t0 = time.time() jnt_traj = instance.compute_trajectory() # return_data flag outputs internal data obtained while computing # the paramterization. This include the time stamps corresponding # to the original waypoints. See below (line 53) to see how to # extract the time stamps. print("Parameterization time: {:} secs".format(time.time() - t0)) instance.compute_feasible_sets() ts_sample = np.linspace(0, jnt_traj.duration, 100) qs_sample = jnt_traj(ts_sample) for i in range(path.dof): # plot the i-th joint trajectory plt.plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i)) # plot the i-th joint waypoints plt.plot(instance.problem_data["t_waypts"], way_pts[:, i], "x", c="C{:d}".format(i)) plt.xlabel("Time (s)") plt.ylabel("Joint position (rad/s^2)") plt.show() K = instance.problem_data['K'] X = instance.problem_data['X'] plt.plot(X[:, 0], c="green", label="Feasible sets") plt.plot(X[:, 1], c="green") plt.plot(K[:, 0], "--", c="red", label="Controllable sets") plt.plot(K[:, 1], "--", c="red") plt.plot(instance.problem_data['sd']**2, label="Velocity profile") plt.title("Path-position path-velocity plot") plt.xlabel("Path position") plt.ylabel("Path velocity square") plt.legend() plt.tight_layout() plt.show()
def test_basic_contact_obj_coincides(setup, si, T_eo, utest, xtest): """Test a simple case where two frames {contact} and {object} coincide. """ robot, ft_name = setup # Inertial properties m = 0.1 lx = 25.2e-3 ly = 62.9e-3 lz = 25.2e-3 I_o = m / 12 * np.diag([ly ** 2 + lz ** 2, lx ** 2 + lz ** 2, lx ** 2 + ly ** 2]) solid_object = transport.SolidObject(robot, ft_name, T_eo, m, I_o) # Contact properties: -1 <= w <= 1 F = np.vstack((np.eye(6), -np.eye(6))) g = np.hstack((np.ones(6), np.ones(6))) contact = transport.Contact(robot, ft_name, T_eo, F, g) pc_object_trans = transport.create_object_transporation_constraint(contact, solid_object, discretization_scheme=0) # TOPPRA parameters waypoints = np.array([ [-0.5, 0.78, 0.78, 0, 0, -0.2], [-0.2, 0.78, 0.78, 0, 0.2, -0.3], [0.2, 0.78, 0.8, 0, 0., 0.4], [0.5, 0.78, 0.78, 0, 0, 0]]) path = ta.SplineInterpolator(np.linspace(0, 1, 4), waypoints) params = pc_object_trans.compute_constraint_params(path, [si])[:5] ai = params[0][0] bi = params[1][0] ci = params[2][0] Fi = params[3] gi = params[4] # Correct coefficient q_ = path.eval(si) qd_ = path.evald(si) * np.sqrt(xtest) qdd_ = path.evaldd(si) * xtest + path.evald(si) * utest T_contact = contact.compute_frame_transform(q_) w_contact = solid_object.compute_inverse_dyn(q_, qd_, qdd_, T_contact) np.testing.assert_allclose(F, Fi) np.testing.assert_allclose(g, gi) np.testing.assert_allclose(ai * utest + bi * xtest + ci, w_contact) np.testing.assert_allclose(Fi.dot(ai * utest + bi * xtest + ci) - gi, Fi.dot(w_contact) - gi)
def get_path_from_trajectory_id(trajectory_id): """ Read and return the path/trajectory by id. """ db = Database() traj_profile = db.retrieve_profile(trajectory_id, "trajectory") # retrieve and define geometric path if "waypoints_npz" in traj_profile: file_ = np.load( os.path.join(db.get_trajectory_data_dir(), traj_profile['waypoints_npz'])) ts_waypoints = file_['t_waypoints'] waypoints = file_['waypoints'] elif "t_waypoints" in traj_profile: ts_waypoints = traj_profile['t_waypoints'] waypoints = traj_profile['waypoints'] else: raise IOError, "Waypoints not found in trajectory {:}".format( traj_profile['id']) # setup toppra instance path = toppra.SplineInterpolator(ts_waypoints, waypoints) return path
def cb(req): print(req) coefficients = ta.SplineInterpolator(req['ss_waypoints'], req['waypoints'], req.get('bc_type', 'not-a-knot')) pc_vel = constraint.JointVelocityConstraint(req['velocity_limits']) pc_acc = constraint.JointAccelerationConstraint( req['acceleration_limits'], discretization_scheme=constraint.DiscretizationType.Interpolation) instance = algo.TOPPRA([pc_vel, pc_acc], coefficients, solver_wrapper='seidel') jnt_traj = instance.compute_trajectory(0, 0) duration = jnt_traj.duration print("Found optimal trajectory with duration {:f} sec".format(duration)) n = coefficients.dof resp = dict(qs=[[]] * n, qds=[[]] * n, qdds=[[]] * n) ts = np.linspace(0, duration, req.get('samples', 100)) for i in range(n): resp['qs'][i] = jnt_traj.eval(ts).tolist() resp['qds'][i] = jnt_traj.evald(ts).tolist() resp['qdds'][i] = jnt_traj.evaldd(ts).tolist() resp['ts'] = ts.tolist() print(resp) return resp
def test_robustness_main(request): """ Load problem suite based on regex, run test and report results. """ toppra.setup_logging(request.config.getoption("--loglevel")) problem_regex = request.config.getoption("--robust_regex") visualize = request.config.getoption("--visualize") # parse problems from a configuration file parsed_problems = [] path = pathlib.Path(__file__) path = path / '../problem_suite_1.yaml' problem_dict = yaml.load(path.resolve().read_text(), Loader=yaml.SafeLoader) for key in problem_dict: if len(problem_dict[key]['ss_waypoints']) == 2: ss_waypoints = np.linspace(problem_dict[key]['ss_waypoints'][0], problem_dict[key]['ss_waypoints'][1], len(problem_dict[key]['waypoints'])) for duration in problem_dict[key]['desired_duration']: for solver_wrapper in problem_dict[key]['solver_wrapper']: for nb_gridpoints in problem_dict[key]['nb_gridpoints']: parsed_problems.append({ "name": key, "problem_id": "{:}-{:5f}-{:}-{:}".format(key, duration, solver_wrapper, nb_gridpoints), 'waypoints': np.array(problem_dict[key]['waypoints'], dtype=float), 'ss_waypoints': ss_waypoints, 'vlim': np.r_[problem_dict[key]['vlim']], 'alim': np.r_[problem_dict[key]['alim']], 'desired_duration': duration, 'solver_wrapper': solver_wrapper, 'gridpoints': np.linspace(ss_waypoints[0], ss_waypoints[-1], nb_gridpoints), 'nb_gridpoints': nb_gridpoints }) parsed_problems_df = pandas.DataFrame(parsed_problems) # solve problems that matched the given regex all_success = True for row_index, problem_data in parsed_problems_df.iterrows(): if re.match(problem_regex, problem_data['problem_id']) is None: continue t0 = time.time() path = toppra.SplineInterpolator( problem_data['ss_waypoints'], problem_data['waypoints'], bc_type='clamped') vlim = np.vstack((- problem_data['vlim'], problem_data['vlim'])).T alim = np.vstack((- problem_data['alim'], problem_data['alim'])).T pc_vel = constraint.JointVelocityConstraint(vlim) pc_acc = constraint.JointAccelerationConstraint( alim, discretization_scheme=constraint.DiscretizationType.Interpolation) t1 = time.time() if problem_data['desired_duration'] == 0: instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=problem_data['gridpoints'], solver_wrapper=problem_data['solver_wrapper']) else: instance = algo.TOPPRAsd([pc_vel, pc_acc], path, gridpoints=problem_data['gridpoints'], solver_wrapper=problem_data['solver_wrapper']) instance.set_desired_duration(problem_data['desired_duration']) t2 = time.time() jnt_traj = instance.compute_trajectory(0, 0) data = instance.problem_data t3 = time.time() if visualize: _t = np.linspace(0, jnt_traj.duration, 100) fig, axs = plt.subplots(2, 2) axs[0, 0].plot(data.K[:, 0], c="C0") axs[0, 0].plot(data.K[:, 1], c="C0") axs[0, 0].plot(data.sd_vec ** 2, c="C1") axs[0, 1].plot(_t, jnt_traj(_t)) axs[1, 0].plot(_t, jnt_traj(_t, 1)) axs[1, 1].plot(_t, jnt_traj(_t, 2)) axs[0, 0].set_title("param") axs[0, 1].set_title("jnt. pos.") axs[1, 0].set_title("jnt. vel.") axs[1, 1].set_title("jnt. acc.") plt.show() if jnt_traj is None: all_success = False parsed_problems_df.loc[row_index, "status"] = "FAIL" parsed_problems_df.loc[row_index, "duration"] = None else: parsed_problems_df.loc[row_index, "status"] = "SUCCESS" parsed_problems_df.loc[row_index, "duration"] = jnt_traj.duration parsed_problems_df.loc[row_index, "t_init(ms)"] = (t1 - t0) * 1e3 parsed_problems_df.loc[row_index, "t_setup(ms)"] = (t2 - t1) * 1e3 parsed_problems_df.loc[row_index, "t_solve(ms)"] = (t3 - t2) * 1e3 # get all rows with status different from NaN, then reports other columns. result_df = parsed_problems_df[parsed_problems_df["status"].notna()][ ["status", "duration", "desired_duration", "name", "solver_wrapper", "nb_gridpoints", "problem_id", "t_init(ms)", "t_setup(ms)", "t_solve(ms)"]] result_df.to_csv('%s.result' % __file__) print("Test summary\n") print(tabulate.tabulate(result_df, result_df.columns)) assert all_success, "Unable to solve some problems in the test suite"