def main(env=None, test=False):
"Main function."
parser = argparse.ArgumentParser(description="A simple example in which trajectories, which are planned using"
"OpenRAVE is retimed using toppra. The trajectories are found using"
"birrt, the default planner. Goals are generated randomly and "
"uniformly.")
parser.add_argument('-e', '--env', help='OpenRAVE Environment file', default="data/lab1.env.xml")
parser.add_argument('-v', '--verbose', help='Show DEBUG log and plot trajectories', action="store_true")
parser.add_argument('-N', '--Ngrid', help='Number of discretization step', default=100, type=int)
args = vars(parser.parse_args())
if args['verbose']:
toppra.setup_logging('DEBUG')
else:
toppra.setup_logging('INFO')
if env is None:
env = orpy.Environment()
env.SetDebugLevel(0)
env.Load(args['env'])
env.SetViewer('qtosg')
robot = env.GetRobots()[0]
robot.SetDOFAccelerationLimits(np.ones(11) * 3)
manipulator = robot.GetManipulators()[0]
robot.SetActiveManipulator(manipulator)
robot.SetActiveDOFs(manipulator.GetArmIndices())
controller = robot.GetController()
basemanip = orpy.interfaces.BaseManipulation(robot)
dof = robot.GetActiveDOF()
pc_torque = toppra.create_rave_torque_path_constraint(
robot, discretization_scheme=toppra.constraint.DiscretizationType.Interpolation)
it = 0
while True or (test and it > 5):
lower, upper = robot.GetActiveDOFLimits()
qrand = np.random.rand(dof) * (upper - lower) + lower
with robot:
robot.SetActiveDOFValues(qrand)
incollision = env.CheckCollision(robot) or robot.CheckSelfCollision()
if incollision:
continue
traj_original = basemanip.MoveActiveJoints(qrand, execute=False, outputtrajobj=True)
if traj_original is None:
continue
traj_retimed, trajra = toppra.retime_active_joints_kinematics(
traj_original, robot, output_interpolator=True, N=args['Ngrid'],
additional_constraints=[pc_torque])
print(("Original duration: {:.3f}. Retimed duration: {:3f}.".format(
traj_original.GetDuration(), traj_retimed.GetDuration())))
if args['verbose']:
plot_trajectories(traj_original, traj_retimed, robot)
time.sleep(1)
controller.SetPath(traj_retimed)
robot.WaitForController(0)
it += 1
def robot_fixture():
env = orpy.Environment()
env.Load("data/lab1.env.xml")
robot = env.GetRobots()[0]
manipulator = robot.GetManipulators()[0]
robot.SetActiveDOFs(manipulator.GetArmIndices())
# Generate IKFast if needed
iktype = orpy.IkParameterization.Type.Transform6D
ikmodel = orpy.databases.inversekinematics.InverseKinematicsModel(robot, iktype=iktype)
if not ikmodel.load():
print 'Generating IKFast {0}. It will take few minutes...'.format(iktype.name)
ikmodel.autogenerate()
print 'IKFast {0} has been successfully generated'.format(iktype.name)
# env.SetViewer('qtosg')
toppra.setup_logging("INFO")
yield robot
env.Destroy()
def main(env=None, test=False):
"Main function."
args = parse_cli_arguments()
toppra.setup_logging('INFO')
robot = load_rave_robot(args)
basemanip = orpy.interfaces.BaseManipulation(robot)
pc_torque = toppra.create_rave_torque_path_constraint(
robot,
discretization_scheme=toppra.constraint.DiscretizationType.
Interpolation)
it = 0
while True or (test and it > 5):
qrand = generate_random_configuration(robot)
traj_original = basemanip.MoveActiveJoints(qrand,
execute=False,
outputtrajobj=True)
if traj_original is None:
continue
traj_retimed, trajra = toppra.retime_active_joints_kinematics(
traj_original,
robot,
output_interpolator=True,
N=args['Ngrid'],
additional_constraints=[pc_torque],
solver_wrapper='seidel')
print("Original duration: {:.3f}. Retimed duration: {:3f}.".format(
traj_original.GetDuration(), traj_retimed.GetDuration()))
robot.GetController().SetPath(traj_retimed)
if args['verbose']:
plot_trajectories(traj_original, traj_retimed, robot)
robot.WaitForController(0)
it += 1
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"
"""This test suite ensures that problem scaling does not affect
solution quality, in the sense that a very short path (~1e-2) and a
very long path (~1e2) can be both parameterized.
"""
import pytest
import numpy as np
import toppra
import toppra.constraint as constraint
toppra.setup_logging(level="INFO")
@pytest.mark.parametrize("solver_wrapper", ["cvxpy", "hotqpoases", "seidel"])
def test_linear_case1(basic_constraints, basic_path, solver_wrapper):
"""A generic test case.
Compare scaling between 0.5 and 1.0. Since the scaling factor is
quite small, resulting trajectories should have similar durations.
"""
vel_c, acc_c, ro_acc_c = basic_constraints
instance_scale1 = toppra.algorithm.TOPPRA(
[vel_c, acc_c], basic_path, solver_wrapper=solver_wrapper, scaling=1.0)
instance_scale05 = toppra.algorithm.TOPPRA(
[vel_c, acc_c], basic_path, solver_wrapper=solver_wrapper, scaling=5)
traj1, _ = instance_scale1.compute_trajectory()
traj05, _ = instance_scale05.compute_trajectory()
# accurate up to 0.1%
np.testing.assert_allclose(traj1.duration, traj05.duration, rtol=1e-3)
import toppra as ta
import toppra.constraint as constraint
import toppra.algorithm as algo
import numpy as np
import matplotlib.pyplot as plt
import argparse
ta.setup_logging("INFO")
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,
"""A test suite for solverwrappers that implement solve methods for
canonical linear constraints. Wrapppers considered include:
'cvxpy', 'qpOASES', "ecos", 'hotqpOASES', 'seidel'.
"""
import pytest
import numpy as np
import numpy.testing as npt
import toppra
import toppra.constraint as constraint
toppra.setup_logging(level="DEBUG")
try:
import cvxpy
FOUND_CXPY = True
except ImportError:
FOUND_CXPY = False
try:
import mosek
FOUND_MOSEK = True
except ImportError:
FOUND_MOSEK = False
class RandomSecondOrderLinearConstraint(constraint.CanonicalLinearConstraint):
"""A random Second-Order non-identical constraint.
This contraint is defined solely for testing purposes. It accepts
def pytest_collection_modifyitems(config, items):
toppra.setup_logging(config.getoption("--loglevel"))
def main():
parser = argparse.ArgumentParser(
description="An example showcasing the usage of robust constraints."
"A velocity constraint and a robust acceleration constraint"
"are considered in this script.")
parser.add_argument("-N",
"--N",
type=int,
help="Number of segments in the discretization.",
default=100)
parser.add_argument("-v", "--verbose", action="store_true", default=False)
parser.add_argument("-du", "--du", default=1e-3, type=float)
parser.add_argument("-dx", "--dx", default=5e-2, type=float)
parser.add_argument("-dc", "--dc", default=9e-3, type=float)
parser.add_argument("-so", "--solver_wrapper", default='ecos')
parser.add_argument("-i", "--interpolation_scheme", default=1, type=int)
args = parser.parse_args()
if args.verbose:
ta.setup_logging("DEBUG")
else:
ta.setup_logging("INFO")
# Parameters
N_samples = 5
dof = 7
# Random waypoints used to obtain a random geometric path.
np.random.seed(9)
way_pts = np.random.randn(N_samples, dof)
# 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
path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts)
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(
alim,
discretization_scheme=constraint.DiscretizationType.Interpolation)
robust_pc_acc = constraint.RobustLinearConstraint(
pc_acc, [args.du, args.dx, args.dc], args.interpolation_scheme)
instance = algo.TOPPRA([pc_vel, robust_pc_acc],
path,
gridpoints=np.linspace(0, 1, args.N + 1),
solver_wrapper=args.solver_wrapper)
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.legend()
plt.title("Path-position path-velocity plot")
plt.show()
jnt_traj, aux_traj = instance.compute_trajectory(0, 0)
ts_sample = np.linspace(0, jnt_traj.duration, 100)
qs_sample = jnt_traj.evaldd(ts_sample)
plt.plot(ts_sample, qs_sample)
plt.show()
parser.add_argument(
"-N",
"--N",
type=int,
help="Number of segments in the discretization.",
default=100,
)
parser.add_argument("-v", "--verbose", action="store_true", default=False)
parser.add_argument("-du", "--du", default=1e-3, type=float)
parser.add_argument("-dx", "--dx", default=5e-2, type=float)
parser.add_argument("-dc", "--dc", default=9e-3, type=float)
parser.add_argument("-so", "--solver_wrapper", default="ecos")
parser.add_argument("-i", "--interpolation_scheme", default=1, type=int)
args = parser.parse_args()
if args.verbose:
ta.setup_logging("DEBUG")
else:
ta.setup_logging("INFO")
# Parameters
N_samples = 5
dof = 7
# Random waypoints used to obtain a random geometric path.
np.random.seed(9)
way_pts = np.random.randn(N_samples, dof)
# 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
import pytest
import toppra
import numpy as np
try:
import openravepy as orpy
FOUND_OPENRAVE = True
except ImportError:
FOUND_OPENRAVE = False
toppra.setup_logging("DEBUG")
@pytest.fixture(scope='module')
def robot_fixture():
env = orpy.Environment()
env.Load("data/lab1.env.xml")
robot = env.GetRobots()[0]
manipulator = robot.GetManipulators()[0]
robot.SetActiveDOFs(manipulator.GetArmIndices())
# Generate IKFast if needed
iktype = orpy.IkParameterization.Type.Transform6D
ikmodel = orpy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=iktype)
if not ikmodel.load():
print('Generating IKFast {0}. It will take few minutes...'.format(
iktype.name))
ikmodel.autogenerate()
print('IKFast {0} has been successfully generated'.format(iktype.name))
# env.SetViewer('qtosg')
toppra.setup_logging("INFO")
yield robot
