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"
