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():
# 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 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 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 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 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 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 _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 compute_spline_varying_alim(self):
"""Compute spline-based jnt_traj one-pass using current alim."""
# avoid going over limit taking into account toppra's precision
pc_vel = constraint.JointVelocityConstraint(self.vlim -
np.sign(self.vlim) *
V_LIM_EPS)
# Can be either Collocation (0) or Interpolation (1).
# Interpolation gives more accurate results with
# slightly higher computational cost
pc_acc = constraint.JointAccelerationConstraint(
self.alim_coeffs.reshape(-1, 1) *
(self.alim - np.sign(self.alim) * A_LIM_EPS),
discretization_scheme=constraint.DiscretizationType.Interpolation,
)
# Since scaling to a shorter path length improves siedel stability,
# prefer short path, try unity next, finally use 1 * t_sum
# which is unlikely to succceed but worth a try anyways if it got there
t_sum_multipliers = [0.03, None, 1]
for multiplier in t_sum_multipliers:
path = self._estimate_path(multiplier, pc_vel, pc_acc)
if (self.qlim is not None):
while self.resplines_allowed > 0:
# If the joint limit checker detects that the spline
# violates joint limits, it will add additional waypts
# to keep the spline within joint limits
if self.joint_limits_obeyed(path, multiplier):
break
logger.info(f"Path violates joint limits. Re-estimating.")
logger.debug(f"waypts = {self.waypts}")
path = self._estimate_path(multiplier, pc_vel, pc_acc)
self.resplines_allowed -= 1
# Use the default gridpoints=None to let
# interpolator.propose_gridpoints calculate gridpoints
# that sufficiently covers the path.
# this ensures the instance is controllable and avoids error:
# "An error occurred when computing controllable velocities.
# The path is not controllable, or is badly conditioned.
# Error: Instance is not controllable"
# If using clamped as boundary condition, the default gridpoints
# error1e-3 is OK and we don't need to calculate gridpoints.
# Boundary condition "natural" especially needs support by
# smaller error.
try:
instance = algo.TOPPRA(
[pc_vel, pc_acc],
path,
solver_wrapper="seidel",
parametrizer="ParametrizeSpline",
)
return self._compute_and_check_traj(
instance, multiplier == t_sum_multipliers[-1])
except RuntimeError:
logger.error(f"t_sum_multiplier = {multiplier} failed")
if multiplier == t_sum_multipliers[-1]:
raise # raise on failure with the last candidate
raise RuntimeError # for linter, never gets here
def vel_accel_robustaccel(request):
"Velocity + Acceleration + Robust Acceleration constraint"
dtype_a, dtype_ra = request.param
vlims = np.array([[-1, 1], [-1, 2], [-1, 4]], dtype=float)
alims = np.array([[-1, 1], [-1, 2], [-1, 4]], dtype=float)
vel_cnst = constraint.JointVelocityConstraint(vlims)
accl_cnst = constraint.JointAccelerationConstraint(alims, dtype_a)
robust_accl_cnst = constraint.RobustCanonicalLinearConstraint(
accl_cnst, [0.5, 0.1, 2.0], dtype_ra)
yield vel_cnst, accl_cnst, robust_accl_cnst
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 basic_constraints(request):
""" Return a set of relatively simple constraints.
"""
dtype_a, dtype_ra = request.param
vlims = np.array([[-1, 1], [-1, 2], [-1, 4], [-3, 4], [-2, 4], [-3, 4], [-2, 5]],
dtype=float) * 10
alims = np.array([[-1, 1], [-1, 2], [-1, 4], [-3, 4], [-2, 4], [-3, 4], [-2, 5]],
dtype=float) * 10
vel_cnst = constraint.JointVelocityConstraint(vlims)
accl_cnst = constraint.JointAccelerationConstraint(alims, dtype_a)
robust_accl_cnst = constraint.RobustCanonicalLinearConstraint(
accl_cnst, [1e-4, 1e-4, 5e-4], dtype_ra)
yield vel_cnst, accl_cnst, robust_accl_cnst
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 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 compute_const_accel(self):
"""Compute optimised trajectory for ParametrizeConstAccel."""
# avoid going over limit taking into account toppra's precision
pc_vel = constraint.JointVelocityConstraint(self.vlim -
np.sign(self.vlim) *
V_LIM_EPS)
pc_acc = constraint.JointAccelerationConstraint(
self.alim - np.sign(self.alim) * A_LIM_EPS,
discretization_scheme=constraint.DiscretizationType.Interpolation,
)
path = self._estimate_path(0.5, pc_vel, pc_acc)
instance = algo.TOPPRA(
[pc_vel, pc_acc],
path,
solver_wrapper="seidel",
parametrizer="ParametrizeConstAccel",
gridpt_min_nb_points=1000, # ensure eps ~ O(1e-2)
)
return self._compute_and_check_traj(instance)
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 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. 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, aux_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.get_duration(), 100)
qs_sample = jnt_traj.eval(ts_sample) # sampled joint positions
qds_sample = jnt_traj.evald(ts_sample) # sampled joint velocities
qdds_sample = jnt_traj.evaldd(ts_sample) # sampled joint accelerations
for i in range(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 generateToppraTrajectoryCallback(self, req):
rospy.logdebug("Generating TOPP-RA trajectory")
tstart = time.time()
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):
rospy.logerr("You must provide at least 2 points to generate a valid trajectory.")
res.success = False
return res
valid = False
p0 = req.waypoints.points[0].positions
for position in req.waypoints.points[1:]:
p1 = position.positions
diff = np.array(p1) - np.array(p0)
if (abs(diff) > 0.0001).any():
valid = True
break
if not valid:
rospy.logerr("All positions are identical. Cannot generate toppra trajectory")
res.success = False
return res
try:
# 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.
rospy.logdebug("Calculating spline interpolation")
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
num_grid_points = np.max([MIN_GRID_POINTS, n*2])
gridpoints = np.linspace(0, path.duration, num_grid_points)
rospy.logdebug("Calling TOPPRA")
instance = algo.TOPPRA([pc_vel, pc_acc], path)
# Retime the trajectory, only this step is necessary.
rospy.logdebug("Computing trajectory from instance")
t0 = time.time()
jnt_traj = instance.compute_trajectory()
rospy.logdebug("Computed traj from instance")
# jnt_traj, aux_traj = instance.compute_trajectory(0, 0)
#print("Parameterization time: {:} secs".format(time.time() - t0))
# Check if trajectory generation was successful
if jnt_traj is None:
rospy.logerr("TOPPRA trajectory generation failed")
res.success = False
return res
# Plot for debugging
if req.plot:
ts_sample = np.linspace(0, jnt_traj.duration, 100)
qs_sample = jnt_traj(ts_sample)
qds_sample = jnt_traj(ts_sample, 1)
qdds_sample = jnt_traj(ts_sample, 2)
fig, axs = plt.subplots(3, 1, sharex=True)
for i in range(path.dof):
# plot the i-th joint trajectory
axs[0].plot(ts_sample, qs_sample[:, i], c="C{:d}".format(i))
axs[1].plot(ts_sample, qds_sample[:, i], c="C{:d}".format(i))
axs[2].plot(ts_sample, qdds_sample[:, i], c="C{:d}".format(i))
axs[2].set_xlabel("Time (s)")
axs[0].set_ylabel("Position (rad)")
axs[1].set_ylabel("Velocity (rad/s)")
axs[2].set_ylabel("Acceleration (rad/s2)")
plt.show()
# Convert to JointTrajectory message
res.trajectory = self.TOPPRA2JointTrajectory(jnt_traj, req.sampling_frequency)
res.success = True
self.raw_trajectory_pub.publish(res.trajectory)
self.raw_waypoints_pub.publish(req.waypoints)
rospy.logdebug("Time elapsed: {}".format(time.time()-tstart))
return res
except Exception as e:
rospy.logderr("Failed to generate TOPPRA traj. Error {}".format(e))
res.success = False
return res
def _generate_movel_trajectory(self, robot_client, rox_robot,
initial_q_or_T, T_final, speed_perc,
q_final_seed):
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)
if isinstance(initial_q_or_T, rox.Robot):
T_initial = initial_q_or_T
q_initial = invkin.update_ik_info3(rox_robot, initial_q_or_T,
np.random.randn((dof, )))
else:
q_initial = initial_q_or_T
T_initial = rox.fwdkin(rox_robot, q_initial)
p_diff = T_final.p - T_initial.p
R_diff = np.transpose(T_initial.R) @ T_final.R
k, theta = rox.R2rot(R_diff)
N_samples_translate = np.linalg.norm(p_diff) * 100.0
N_samples_rot = np.abs(theta) * 0.25 * 180 / np.pi
N_samples_translate = np.linalg.norm(p_diff) * 100.0
N_samples_rot = np.abs(theta) * 0.2 * 180 / np.pi
N_samples = math.ceil(np.max((N_samples_translate, N_samples_rot, 20)))
ss = np.linspace(0, 1, N_samples)
if q_final_seed is None:
q_final, res = invkin.update_ik_info3(rox_robot, T_final,
q_initial)
else:
q_final, res = invkin.update_ik_info3(rox_robot, T_final,
q_final_seed)
#assert res, "Inverse kinematics failed"
way_pts = np.zeros((N_samples, dof), dtype=np.float64)
way_pts[0, :] = q_initial
for i in range(1, N_samples):
s = float(i) / (N_samples - 1)
R_i = T_initial.R @ rox.rot(k, theta * s)
p_i = (p_diff * s) + T_initial.p
T_i = rox.Transform(R_i, p_i)
#try:
# q, res = invkin.update_ik_info3(rox_robot, T_i, way_pts[i-1,:])
#except AssertionError:
ik_seed = (1.0 - s) * q_initial + s * q_final
q, res = invkin.update_ik_info3(rox_robot, T_i, ik_seed)
assert res, "Inverse kinematics failed"
way_pts[i, :] = q
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 main():
# openrave setup
env = orpy.Environment()
env.Load("robots/barrettwam.robot.xml")
env.SetViewer('qtosg')
robot = env.GetRobots()[0]
robot.SetActiveDOFs(range(7))
# 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) * 0.6
path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts)
# Create velocity bounds, then velocity constraint object
vlim_ = robot.GetActiveDOFMaxVel()
vlim_[robot.GetActiveDOFIndices()] = [80., 80., 80., 80., 80., 80., 80.]
vlim = np.vstack((-vlim_, vlim_)).T
# Create acceleration bounds, then acceleration constraint object
alim_ = robot.GetActiveDOFMaxAccel()
alim_[robot.GetActiveDOFIndices()] = [80., 80., 80., 80., 80., 80., 80.]
alim = np.vstack((-alim_, alim_)).T
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(
alim,
discretization_scheme=constraint.DiscretizationType.Interpolation)
# torque limit
def inv_dyn(q, qd, qdd):
qdd_full = np.zeros(robot.GetDOF())
active_dofs = robot.GetActiveDOFIndices()
with robot:
# Temporary remove vel/acc constraints
vlim = robot.GetDOFVelocityLimits()
alim = robot.GetDOFAccelerationLimits()
robot.SetDOFVelocityLimits(100 * vlim)
robot.SetDOFAccelerationLimits(100 * alim)
# Inverse dynamics
qdd_full[active_dofs] = qdd
robot.SetActiveDOFValues(q)
robot.SetActiveDOFVelocities(qd)
res = robot.ComputeInverseDynamics(qdd_full)
# Restore vel/acc constraints
robot.SetDOFVelocityLimits(vlim)
robot.SetDOFAccelerationLimits(alim)
return res[active_dofs]
tau_max_ = robot.GetDOFTorqueLimits() * 4
tau_max = np.vstack((-tau_max_[robot.GetActiveDOFIndices()],
tau_max_[robot.GetActiveDOFIndices()])).T
fs_coef = np.random.rand(dof) * 10
pc_tau = constraint.JointTorqueConstraint(
inv_dyn,
tau_max,
fs_coef,
discretization_scheme=constraint.DiscretizationType.Interpolation)
all_constraints = [pc_vel, pc_acc, pc_tau]
# all_constraints = pc_vel
instance = algo.TOPPRA(all_constraints, path, solver_wrapper='seidel')
# Retime the trajectory, only this step is necessary.
t0 = time.time()
jnt_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)
torque = []
for q_, qd_, qdd_ in zip(qs_sample, qds_sample, qdds_sample):
torque.append(inv_dyn(q_, qd_, qdd_) + fs_coef * np.sign(qd_))
torque = np.array(torque)
fig, axs = plt.subplots(dof, 1)
for i in range(0, robot.GetActiveDOF()):
axs[i].plot(ts_sample, torque[:, i])
axs[i].plot([ts_sample[0], ts_sample[-1]], [tau_max[i], tau_max[i]],
"--")
axs[i].plot([ts_sample[0], ts_sample[-1]], [-tau_max[i], -tau_max[i]],
"--")
plt.xlabel("Time (s)")
plt.ylabel("Torque $(Nm)$")
plt.legend(loc='upper right')
plt.show()
# preview path
for t in np.arange(0, jnt_traj.get_duration(), 0.01):
robot.SetActiveDOFValues(jnt_traj.eval(t))
time.sleep(0.01) # 5x slow down
# 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 move_move_no(limb, group, target, speed_ratio=None, accel_ratio=None, timeout=None):
trajectory_publisher = rospy.Publisher('/robot/limb/right/joint_command', JointCommand, queue_size = 1)
JointCommandMessage = JointCommand()
JointCommandMessage.mode = 4
if speed_ratio is None:
speed_ratio = 0.3
if girigiri_aiiiiii:
speed_ratio = 0.8
if accel_ratio is None:
accel_ratio = 0.1
if girigiri_aiiiiii:
accel_ratio = 0.2
plan = group.plan(target)
rospy.sleep(1)
JointCommandMessage.names = plan.joint_trajectory.joint_names
start_time = rospy.get_time()
position_array = []
velocity_array = []
acceleration_array = []
time_array = []
for point in plan.joint_trajectory.points:
position = point.positions
velocity = point.velocities
acceleration = point.accelerations
position_array.append(position)
velocity_array.append(velocity)
acceleration_array.append(acceleration)
desire_time = point.time_from_start.to_sec()
time_array.append(desire_time)
s_array = time_array
wp_array = position_array
path = ta.SplineInterpolator(s_array, wp_array)
s_sampled = np.linspace(0, time_array[len(time_array) - 1], 100)
q_sampled = path.eval(s_sampled)
# --------------------------- plotting the interpolator --------------------------
# plt.plot(s_sampled, q_sampled)
# plt.hold(True)
# plt.plot(time_array, position_array, 'kd')
# plt.legend(["joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7"])
# plt.hold(False)
# plt.show()
# exit()
# ---------------------------- end of plotting ----------------------------------
# Create velocity bounds, then velocity constraint object
dof = 7
vlim_ = np.ones(dof) * 5 #* speed_ratio
vlim = np.vstack((-vlim_, vlim_)).T
# Create acceleration bounds, then acceleration constraint object
alim_ = np.ones(dof) * 2 * accel_ratio
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("TOPPRA Parameterization time: {:} secs".format(time.time() - t0))
ts_sample = np.linspace(0, jnt_traj.get_duration(), 800)
position_output = jnt_traj.eval(ts_sample)
velocity_output = jnt_traj.evald(ts_sample)
acceleration_output = jnt_traj.evaldd(ts_sample)
# --------------------------- plotting the algorithm output ---------------------------
# plt.subplot(3,1,1)
# plt.plot(ts_sample, position_output)
# plt.subplot(3,1,2)
# plt.plot(ts_sample, velocity_output)
# plt.subplot(3,1,3)
# plt.plot(ts_sample, acceleration_output)
# plt.show()
# --------------------------- end plot the algorithm output ---------------------------
start_time = rospy.get_time()
for i in range(len(ts_sample) - 1):
JointCommandMessage.position = position_output[i]
JointCommandMessage.velocity = velocity_output[i]
JointCommandMessage.acceleration = acceleration_output[i]
desire_time = ts_sample[i + 1]
while (rospy.get_time() - start_time) < desire_time:
trajectory_publisher.publish(JointCommandMessage)
JointCommandMessage.position = position_output[-1]
JointCommandMessage.velocity = velocity_output[-1]
JointCommandMessage.acceleration = acceleration_output[-1]
trajectory_publisher.publish(JointCommandMessage)
def main():
# Setup the waypoints here. Quad without manipulator has 4 degrees of
# freedom. We will use (x,y,z,yaw) in this example. Each degree of freedom
# has N waypoints and we have to arrange those as numpy arrays. Let's try
# a square set of waypoints
way_pts = np.array([[0, 0, 1, 0], [0.5, 0, 1, 0], [1, 0, 1, 0],
[1, 0.5, 1, 0], [1, 1, 1, 0], [0.5, 1, 1, 0],
[0, 1, 1, 0], [0, 0.5, 1, 0], [0, 0, 1, 0]])
# linspace goes from 0 to 1 with 9 samples. This is because we have
# 9 waypoints.
path = ta.SplineInterpolator(np.linspace(0, 1, 9), way_pts)
# Create velocity bounds, then velocity constraint object
vlim_low = np.array([-2, -2, -2, -1])
vlim_high = np.array([2, 2, 2, 1])
vlim = np.vstack((vlim_low, vlim_high)).T
# Create acceleration bounds, then acceleration constraint object
alim_low = np.array([-2, -2, -2, -1])
alim_high = np.array([2, 2, 2, 1])
alim = np.vstack((alim_low, alim_high)).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))
# Sampling frequency is required to get the time samples correctly.
# The number of points in ts_sample is duration*frequency.
sample_freq = 100
ts_sample = np.linspace(0, jnt_traj.get_duration(),
int(jnt_traj.get_duration() * sample_freq))
# Sampling. This returns a matrix for all DOFs. Accessing specific one is
# simple: qs_sample[:, 0]
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()
plt.plot(qs_sample[:, 0], qs_sample[:, 1])
plt.show()
plt.plot(ts_sample, qs_sample[:, 1])
plt.show()
def interpolate_by_max_spdacc(self,
path,
control_frequency=.005,
max_vels=None,
max_accs=None,
toggle_debug_fine=False,
toggle_debug=True):
"""
TODO: prismatic motor speed is not considered
:param path:
:param control_frequency:
:param max_vels: max jnt speed between two adjacent poses in the path, math.pi if None
:param max_accs: max jnt speed between two adjacent poses in the path, math.pi if None
:return:
author: weiwei
date: 20210712, 20211012
"""
path = self._remove_duplicate(path)
self._path_array = np.array(path)
self._n_pnts, _ = self._path_array.shape
if max_vels is None:
max_vels = [math.pi * 2 / 3] * path[0].shape[0]
if max_accs is None:
max_accs = [math.pi] * path[0].shape[0]
max_vels = np.asarray(max_vels)
max_accs = np.asarray(max_accs)
# initialize seed time inervals
time_intervals = []
for i in range(self._n_pnts - 1):
pose_diff = abs(path[i + 1] - path[i])
tmp_time_interval = np.max(pose_diff / max_vels)
time_intervals.append(tmp_time_interval)
time_intervals = np.array(time_intervals)
x = [0]
tmp_total_x = 0
for i in range(len(time_intervals)):
tmp_time_interval = time_intervals[i]
x.append(tmp_time_interval + tmp_total_x)
tmp_total_x += tmp_time_interval
interpolated_path = ta.SplineInterpolator(x, path)
pc_vel = constraint.JointVelocityConstraint(max_vels)
pc_acc = constraint.JointAccelerationConstraint(max_accs)
instance = algo.TOPPRA([pc_vel, pc_acc], interpolated_path)
jnt_traj = instance.compute_trajectory()
duration = jnt_traj.duration
print(
"Found optimal trajectory with duration {:f} sec".format(duration))
ts = np.linspace(0, duration, math.ceil(duration / control_frequency))
interpolated_confs = jnt_traj.eval(ts)
interpolated_spds = jnt_traj.evald(ts)
interpolated_accs = jnt_traj.evaldd(ts)
if toggle_debug:
import matplotlib.pyplot as plt
fig, axs = plt.subplots(3, figsize=(10, 30))
fig.tight_layout(pad=.7)
# curve
axs[0].plot(ts, interpolated_confs, 'o')
# speed
axs[1].plot(ts, interpolated_spds)
for ys in max_vels:
axs[1].axhline(y=ys)
axs[1].axhline(y=-ys)
# acceleration
axs[2].plot(ts, interpolated_accs)
for ys in max_accs:
axs[2].axhline(y=ys)
axs[2].axhline(y=-ys)
plt.show()
return interpolated_confs
np.random.seed(seed)
way_pts = np.random.randn(N_samples, dof)
return (
np.linspace(0, 1, 5),
way_pts,
10 + np.random.rand(dof) * 20,
10 + np.random.rand(dof) * 2,
)
ss, way_pts, vlims, alims = generate_new_problem()
################################################################################
# Define the geometric path and two constraints.
path = ta.SplineInterpolator(ss, way_pts)
pc_vel = constraint.JointVelocityConstraint(vlims)
pc_acc = constraint.JointAccelerationConstraint(alims)
################################################################################
# We solve the parametrization problem using the
# `ParametrizeConstAccel` parametrizer. This parametrizer is the
# classical solution, guarantee constraint and boundary conditions
# satisfaction.
instance = algo.TOPPRA([pc_vel, pc_acc],
path,
parametrizer="ParametrizeConstAccel")
jnt_traj = instance.compute_trajectory()
################################################################################
# The output trajectory is an instance of
# :class:`toppra.interpolator.AbstractGeometricPath`.
def main():
# openrave setup
env = orpy.Environment()
env.Load("robots/barrettwam.robot.xml")
env.SetViewer('qtosg')
robot = env.GetRobots()[0]
robot.SetActiveDOFs(range(7))
# 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) * 0.6
path = ta.SplineInterpolator(np.linspace(0, 1, 5), way_pts)
# Create velocity bounds, then velocity constraint object
vlim_ = robot.GetActiveDOFMaxVel()
vlim = np.vstack((-vlim_, vlim_)).T
# Create acceleration bounds, then acceleration constraint object
alim_ = robot.GetActiveDOFMaxAccel()
alim = np.vstack((-alim_, alim_)).T
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(
alim, discretization_scheme=constraint.DiscretizationType.Interpolation)
# setup cartesian acceleration constraint to limit link 7
# -0.5 <= a <= 0.5
# cartersian acceleration
def inverse_dynamics(q, qd, qdd):
with robot:
vlim_ = robot.GetDOFVelocityLimits()
robot.SetDOFVelocityLimits(vlim_ * 1000) # remove velocity limits to compute stuffs
robot.SetActiveDOFValues(q)
robot.SetActiveDOFVelocities(qd)
qdd_full = np.zeros(robot.GetDOF())
qdd_full[:qdd.shape[0]] = qdd
accel_links = robot.GetLinkAccelerations(qdd_full)
robot.SetDOFVelocityLimits(vlim_)
return accel_links[6][:3] # only return the translational components
F_q = np.zeros((6, 3))
F_q[:3, :3] = np.eye(3)
F_q[3:, :3] = -np.eye(3)
g_q = np.ones(6) * 0.5
def F(q):
return F_q
def g(q):
return g_q
pc_cart_acc = constraint.CanonicalLinearSecondOrderConstraint(
inverse_dynamics, F, g, dof=7)
# cartesin accel finishes
all_constraints = [pc_vel, pc_acc, pc_cart_acc]
instance = algo.TOPPRA(all_constraints, 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)
cart_accel = []
for q_, qd_, qdd_ in zip(qs_sample, qds_sample, qdds_sample):
cart_accel.append(inverse_dynamics(q_, qd_, qdd_))
cart_accel = np.array(cart_accel)
plt.plot(ts_sample, cart_accel[:, 0], label="$a_x$")
plt.plot(ts_sample, cart_accel[:, 1], label="$a_y$")
plt.plot(ts_sample, cart_accel[:, 2], label="$a_z$")
plt.plot([ts_sample[0], ts_sample[-1]], [0.5, 0.5], "--", c='black', label="Cart. Accel. Limits")
plt.plot([ts_sample[0], ts_sample[-1]], [-0.5, -0.5], "--", c='black')
plt.xlabel("Time (s)")
plt.ylabel("Cartesian acceleration of the origin of link 6 $(m/s^2)$")
plt.legend(loc='upper right')
plt.show()
# preview path
for t in np.arange(0, jnt_traj.get_duration(), 0.01):
robot.SetActiveDOFValues(jnt_traj.eval(t))
time.sleep(0.01) # 5x slow down
# 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 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()
def generateToppraTrajectoryCallback(self, req):
print(" ")
print("Generating TOPP-RA trajectory.")
tstart = time.time()
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
if (req.n_gridpoints <= 0):
num_grid_points = np.max([100, n * 2])
gridpoints = np.linspace(0, path.duration, num_grid_points)
else:
gridpoints = np.linspace(0, path.duration, req.n_gridpoints)
instance = algo.TOPPRA([pc_vel, pc_acc],
path,
gridpoints=gridpoints,
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))
# Plot for debugging
if req.plot == True:
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()
plt.plot(qs_sample[:, 0], qs_sample[:, 1])
plt.show()
# Convert to JointTrajectory message
res.trajectory = self.TOPPRA2JointTrajectory(jnt_traj,
req.sampling_frequency)
res.success = True
if len(req.waypoints.joint_names) != 0:
res.trajectory.joint_names = copy.deepcopy(
req.waypoints.joint_names)
self.raw_trajectory_pub.publish(res.trajectory)
self.raw_waypoints_pub.publish(req.waypoints)
print("Time elapsed: ", time.time() - tstart)
return res
def traj_reparam(compas_fab_jt_traj,
max_jt_vel,
max_jt_acc,
traj_time_cnt=0,
ts_sample_num=100,
grid_num=200,
inspect_sol=False):
dof = len(compas_fab_jt_traj.points[0].values)
# Create path
way_jt_pts = [jt_pt.values for jt_pt in compas_fab_jt_traj.points]
path = ta.SplineInterpolator(
np.linspace(0, 1, len(compas_fab_jt_traj.points)), way_jt_pts)
# Create velocity bounds, then velocity constraint object
vlim_ = np.ones(6) * max_jt_vel
vlim = np.vstack((-vlim_, vlim_)).T
# Create acceleration bounds, then acceleration constraint object
alim_ = np.ones(6) * max_jt_acc
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, then retime
gridpoints = np.linspace(0, path.duration, grid_num)
instance = algo.TOPPRA([pc_vel, pc_acc],
path,
gridpoints=gridpoints,
solver_wrapper='seidel')
jnt_traj, aux_traj, int_data = instance.compute_trajectory(
0, 0, return_data=True)
if inspect_sol:
ts_sample = np.linspace(0, jnt_traj.get_duration(), 100)
qdds_sample = jnt_traj.evaldd(ts_sample)
qds_sample = jnt_traj.evald(ts_sample)
fig, axs = plt.subplots(1, 2, sharex=True, figsize=[12, 4])
for i in range(6):
axs[0].plot(ts_sample,
qdds_sample[:, i],
label="J{:d}".format(i + 1))
axs[1].plot(ts_sample,
qds_sample[:, i],
label="J{:d}".format(i + 1))
axs[0].set_xlabel("Time (s)")
axs[0].set_ylabel("Joint acceleration (rad/s^2)")
axs[0].legend()
axs[1].legend()
axs[1].set_xlabel("Time (s)")
axs[1].set_ylabel("Joint velocity (rad/s)")
plt.show()
time_list = np.linspace(0, float(jnt_traj.get_duration()), ts_sample_num)
jt_list = jnt_traj.eval(time_list)
vel_list = jnt_traj.evald(time_list)
acc_list = jnt_traj.evaldd(time_list)
reparm_traj_pts = []
for i in range(ts_sample_num):
new_jt_pt = JointTrajectoryPoint(values=jt_list[i].tolist(),
types=[0] * 6)
new_jt_pt.velocities = vel_list[i].tolist()
new_jt_pt.accelerations = acc_list[i].tolist()
new_jt_pt.time_from_start = Duration.from_seconds(traj_time_cnt)
traj_time_cnt += time_list[i]
reparm_traj_pts.append(new_jt_pt)
if i == 0 and np.array_equal(time_list, np.zeros(ts_sample_num)):
break
reparam_traj = JointTrajectory(trajectory_points=reparm_traj_pts,
start_configuration=reparm_traj_pts[0])
return reparam_traj
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"
def call_TOPPRA(req):
### Get the request path and prepare for TOPP-RA
t0 = time.time()
print("= Get the request path...")
path_plan = req.path
vel_limits = req.vel_limits
acc_limits = req.acc_limits
dt = req.timestep
# kinematic constraints (and constraint object)
vlim = np.vstack((-np.array(vel_limits), vel_limits)).T # list is not an operand type for unary
alim = np.vstack((-np.array(acc_limits), acc_limits)).T
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(alim, discretization_scheme=constraint.DiscretizationType.Interpolation)
# convert to waypoint array
print("== Convert to an array of waypoints...")
dof = len(path_plan[0].positions)
ndata = len(path_plan)
path_array = np.zeros((ndata, dof))
for i in range(ndata):
path_array[i, :] = np.array(path_plan[i].positions)
path = ta.SplineInterpolator(np.linspace(0, 1, ndata), path_array)
### Setup a TOPP-RA instance
t1 = time.time()
print("=== Set up a TOPP-RA instance and run the algorithm...")
gridpoints = np.linspace(0, path.duration, 400) #3000) #2000) #1000) #800) #400) #200) # why grid points??? it doesn't seem to have anything to do with the result...
instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=gridpoints, solver_wrapper='seidel')
# different solver wrappers -- 'seidel'(fastest), 'hotqpoases'(second fastest), 'cvxpy'
# 'seidel' can fail for ill-posed path parameterization instances
t2 = time.time()
jnt_traj, aux_traj, int_data = instance.compute_trajectory(0, 0, return_data=True)
### Run TOPP-RA to get the result
t3 = time.time()
print("==== Get the resultant pos, vel, acc and time profiles...")
# get time sequence
t = 0.0
t_seq = []
while t < jnt_traj.get_duration():
t_seq.append(t)
t += dt
t_seq.append(jnt_traj.get_duration())
# get positions, velocities and accelerations
q_seq = jnt_traj.eval(t_seq)
qd_seq = jnt_traj.evald(t_seq)
qdd_seq = jnt_traj.evaldd(t_seq)
# convert to plan and return
print("===== Convert to plan and return...")
traj_point = JointTrajectoryPoint()
traj_points = [] #PathToTrajResponse() # just an empty list
for i in range(len(t_seq)):
traj_point.positions = q_seq[i, :].tolist()
traj_point.velocities = qd_seq[i, :].tolist()
traj_point.accelerations = qdd_seq[i, :].tolist()
traj_point.time_from_start = rospy.Duration(t_seq[i])
traj_points.append(copy.deepcopy(traj_point))
t4 = time.time()
### Display statistics
print(">>>>> Statistics of TOPP-RA <<<<<")
print("Dimension of path point: " + str(dof))
print("Number of data points: " + str(ndata))
print("Obtain request path and get prepared: " + str(t1-t0) + " s")
print("Setup TOPP-RA instance: " + str(t2-t1) + " s")
print("Run TOPP-RA to compute the trajectory: " + str(t3-t2) + " s")
print("Get the results and convert to plan: " + str(t4-t3) + " s")
print("Total: " + str(t4-t0) + " s")
print(">>>>> End <<<<<")
### Return result
res = PathToTrajResponse()
res.traj = traj_points
return res