def test_cubic(self):
t = [0., 1., 2.]
x = np.diag((4., 5., 6.))
# Just test the spelling for these.
pp1 = PiecewisePolynomial.Cubic(t, x)
pp2 = PiecewisePolynomial.Cubic(t, x, np.identity(3))
pp3 = PiecewisePolynomial.Cubic(t, x, [0., 0., 0.], [0., 0., 0.])
def test_direct_transcription(self):
# Integrator.
plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.], time_period=0.1)
context = plant.CreateDefaultContext()
dirtran = DirectTranscription(plant, context, num_time_samples=21)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dirtran.time()
dt = dirtran.fixed_timestep()
x = dirtran.state()
x2 = dirtran.state(2)
x0 = dirtran.initial_state()
xf = dirtran.final_state()
u = dirtran.input()
u2 = dirtran.input(2)
dirtran.AddRunningCost(x.dot(x))
dirtran.AddConstraintToAllKnotPoints(u[0] == 0)
dirtran.AddFinalCost(2 * x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3 * 21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dirtran.SetInitialTrajectory(initial_u, initial_x)
result = mp.Solve(dirtran.prog())
times = dirtran.GetSampleTimes(result)
inputs = dirtran.GetInputSamples(result)
states = dirtran.GetStateSamples(result)
input_traj = dirtran.ReconstructInputTrajectory(result)
state_traj = dirtran.ReconstructStateTrajectory(result)
def test_compare_and_concatenate(self):
x = np.array([[10.], [20.], [30.]]).transpose()
pp1 = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
pp2 = PiecewisePolynomial.FirstOrderHold([2., 3., 4.], x)
self.assertTrue(pp1.isApprox(other=pp1, tol=1e-14))
pp1.ConcatenateInTime(other=pp2)
self.assertEqual(pp1.end_time(), 4.)
def test_piecewise_polynomial_matrix_constructor(self):
pm1 = np.array([[Polynomial(1), Polynomial(2)]])
pm2 = np.array([[Polynomial(2), Polynomial(0)]])
pp = PiecewisePolynomial([pm1, pm2], [0, 1, 2])
numpy_compare.assert_equal(pp.getPolynomialMatrix(segment_index=0),
pm1)
pm3 = np.array([[Polynomial(5), Polynomial(10)]])
pp.setPolynomialMatrixBlock(replacement=pm3, segment_index=1)
def generate_rpyxyz_and_gripper_trajectory(self, mbp_context):
start_position = self.mbp.EvalBodyPoseInWorld(
mbp_context, self.target_body).translation()
adjusted_goal_position = self.goal_position.copy()
if self.offset_body:
adjusted_goal_position += self.mbp.EvalBodyPoseInWorld(
mbp_context, self.offset_body).translation()
start_ee_pose = self.mbp.EvalBodyPoseInWorld(
mbp_context, self.mbp.GetBodyByName("iiwa_link_7"))
grasp_offset = np.array(
[0.0, 0., 0.225]) # Amount *all* target points are shifted up
up_offset = np.array(
[0., 0., 0.1]) # Additional amount to lift objects off of table
# Timing:
# 0 : t_reach: move over object
# t_reach : t_touch: move down to object
# t_touch : t_grasp: close gripper
# t_grasp : t_lift : pick up object
# t_lift : t_move : move object over destination
# t_move : t_down : move object down
# t_down : t_drop : open gripper
# t_drop : t_done : rise back up
t_each = 0.25
t_reach = 0. + t_each
t_touch = t_reach + t_each
t_grasp = t_touch + t_each
t_lift = t_grasp + t_each
t_move = t_lift + t_each
t_down = t_move + t_each
t_drop = t_down + t_each
t_done = t_drop + t_each
ts = np.array([
0., t_reach, t_touch, t_grasp, t_lift, t_move, t_down, t_drop,
t_done
])
ee_xyz_knots = np.vstack([
start_ee_pose.translation() - grasp_offset,
start_position + up_offset,
start_position,
start_position,
start_position + up_offset,
adjusted_goal_position + up_offset,
adjusted_goal_position,
adjusted_goal_position,
adjusted_goal_position + up_offset,
]).T
ee_xyz_knots += np.tile(grasp_offset, [len(ts), 1]).T
ee_rpy_knots = np.array([-np.pi, 0., np.pi / 2.])
ee_rpy_knots = np.tile(ee_rpy_knots, [len(ts), 1]).T
ee_rpyxyz_knots = np.vstack([ee_rpy_knots, ee_xyz_knots])
ee_rpyxyz_traj = PiecewisePolynomial.FirstOrderHold(
ts, ee_rpyxyz_knots)
gripper_knots = np.array([[0.1, 0.1, 0.1, 0., 0., 0., 0., 0.1, 0.1]])
gripper_traj = PiecewisePolynomial.FirstOrderHold(ts, gripper_knots)
return ee_rpyxyz_traj, gripper_traj
def test_cubic(self):
t = [0., 1., 2.]
x = np.diag([4., 5., 6.])
periodic_end = False
# Just test the spelling for these.
pp1 = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t, samples=x, periodic_end=periodic_end)
pp2 = PiecewisePolynomial.CubicHermite(
breaks=t, samples=x, samples_dot=np.identity(3))
pp3 = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t, samples=x, sample_dot_at_start=[0., 0., 0.],
sample_dot_at_end=[0., 0., 0.])
def urdfViz(plant):
"""
Visualize the position/orientation of the vehicle along a trajectory using the PlanarRigidBodyVisualizer and
a basic .urdf file representing the vehicle, start, goal, and state constraints.
"""
tree = RigidBodyTree(FindResource("/notebooks/6832-code/ben_uav.urdf"),
FloatingBaseType.kRollPitchYaw)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-1, 15], ylim=[-0.5, 2.5])
tf = plant.ttraj[-1]
times = np.linspace(0, tf, 200)
# organize the relevant states for the visualizer
posn = np.zeros((13, len(times)))
for i, t in enumerate(times):
x = plant.xdtraj_poly.value(t)
u = plant.udtraj_poly.value(t)
posn[0:6, i] = 0
posn[6, i] = (x[0] - plant.xdtraj[:,0].min()) / 14 # x
posn[7, i] = 0 # z
posn[8, i] = x[1] # y
posn[9, i] = 0 # yz plane
posn[10, i] = np.pi / 2 - x[4] # pitch down
posn[11, i] = 0 # yaw
posn[12, i] = -u[1] # tail angle
test_poly = PiecewisePolynomial.FirstOrderHold(times, posn)
return vis.animate(test_poly, repeat=False)
def test_hermite(self):
t = [0., 1., 2.]
x = np.array([[0, 1, 1]])
pp = PiecewisePolynomial.CubicShapePreserving(
breaks=t, samples=x, zero_end_point_derivatives=False)
pp.AppendCubicHermiteSegment(time=3., sample=[2], sample_dot=[2])
pp.RemoveFinalSegment()
def test_trajectory(self):
builder = DiagramBuilder()
visualizer = builder.AddSystem(TestVisualizer(1))
ppt = PiecewisePolynomial.FirstOrderHold(
[0., 1.], [[2., 3.], [2., 1.]])
ani = visualizer.animate(ppt)
self.assertIsInstance(ani, animation.FuncAnimation)
def experiment(start_state, force_schedule, sleeptime=None):
global experiment_count
experiment_count += 1
if experiment_count % 10 == 0:
print(f"{experiment_count} experiments run so far...")
builder = DiagramBuilder()
mbp, sg = make_mbp(builder)
mbp.Finalize()
DrakeVisualizer.AddToBuilder(builder, sg, lcm=global_lcm_ftw)
thrusters = add_thrusters(builder, mbp)
breaks = [0]
for _, t in force_schedule:
breaks.append(breaks[-1] + t)
forces = np.array([f for f, t in force_schedule] + [force_schedule[-1][0]])
force_traj = PiecewisePolynomial.ZeroOrderHold(breaks, forces.transpose())
controller = builder.AddSystem(TrajectorySource(force_traj))
builder.Connect(controller.get_output_port(),
thrusters.get_command_input_port())
diagram = builder.Build()
simulator = Simulator(diagram)
mbp_context = diagram.GetSubsystemContext(mbp, simulator.get_context())
mbp.SetPositionsAndVelocities(mbp_context, start_state)
for step in range(int(1000 * breaks[-1])):
simulator.AdvanceTo(0.001 * step)
mbp_context = diagram.GetSubsystemContext(mbp, simulator.get_context())
if sleeptime: time.sleep(sleeptime)
return mbp.GetPositionsAndVelocities(
diagram.GetSubsystemContext(mbp, simulator.get_context()))
def MoveEndEffectorAlongStraightLine(
p_WQ_start, p_WQ_end, duration, get_orientation, q_initial_guess, num_knots):
"""
p_WQ_start is the starting position of point Q (the point Q we defined earlier!)
p_WQ_end is the end position of point Q.
duration is the duration of the trajectory measured in seconds.
get_orientation(i, n) is a function that returns the interpolated orientation R_WL7 at the i-th knot point out of n knot points. It can simply return a constant orientation, but changing the orientation of link 7 should be helpful for placing the object on the shelf.
num_knots is the number of knot points in the trajectory. Generally speaking,the larger num_knots is, the smoother the trajectory, but solving for more knot points also takes longer. You can start by setting num_knots to 10, which should be sufficient for the task.
"""
t_knots = np.linspace(0, duration, num_knots+1)
q_knots = np.zeros((num_knots, plant.num_positions()))
q_knots[0] = q_initial_guess
n = len(q_initial_guess)
for i in range(num_knots):
ik = inverse_kinematics.InverseKinematics(plant)
prog = ik.prog()
p_AQ = p_WQ_start + (p_WQ_end - p_WQ_start) * i / (num_knots - 1)
ik.AddPositionConstraint(frameB=l7_frame, p_BQ=p_L7Q, frameA=world_frame, p_AQ_lower=p_AQ - 0.01, p_AQ_upper=p_AQ + 0.01) # interpolate between p_WQ_start and p_WQ_end
ik.AddOrientationConstraint(frameAbar=world_frame, R_AbarA=get_orientation(i, num_knots), frameBbar=l7_frame, R_BbarB=RotationMatrix(), theta_bound=theta_bound) # call get_orientation(i, num_knots).
if i == 0:
prog.SetInitialGuess(ik.q(), q_initial_guess)
else:
# This is very important for the smoothness of the whole trajectory.
prog.SetInitialGuess(ik.q(), q_knots[i - 1])
result = mp.Solve(ik.prog())
assert result.get_solution_result() == mp.SolutionResult.kSolutionFound
q_knots[i] = result.GetSolution(ik.q())
return PiecewisePolynomial.Cubic(t_knots[1:], q_knots.T, np.zeros(n), np.zeros(n)) # return a cubic spline that connects all q_knots.
def GeneratePickupBrickPlans(p_goal, is_printing=True):
def InterpolateStraightLine(p_WQ_start, p_WQ_end, num_knot_points, i):
return (p_WQ_end - p_WQ_start) / num_knot_points * (i + 1) + p_WQ_start
num_knot_points = 10
q_home_full = GetHomeConfiguration(is_printing)
p_WQ_start = p_WQ_home
p_WQ_end = p_goal
qtraj_move_to_box, q_knots_full = InverseKinPointwise(
p_WQ_start,
p_WQ_end,
duration=5.0,
num_knot_points=num_knot_points,
q_initial_guess=q_home_full,
InterpolatePosition=InterpolateStraightLine,
position_tolerance=0.001,
is_printing=is_printing)
# close gripper
q_knots = np.zeros((2, 7))
q_knots[0] = qtraj_move_to_box.value(
qtraj_move_to_box.end_time()).squeeze()
qtraj_close_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)
q_traj_list = [qtraj_move_to_box, qtraj_close_gripper]
plan_list = []
for q_traj in q_traj_list:
plan_list.append(JointSpacePlan(q_traj))
gripper_setpoint_list = [0.1, 0.005]
q_final_full = q_knots_full[-1]
return plan_list, gripper_setpoint_list, q_final_full
def rrt(self, star=False, max_iters=1000, goal_sample_prob=0.05, position_tolerance=0.09, duration=5):
""" Motion Planning by RRT and RRT*
:param star: True if RRT*
:param max_iters: max number of iterations
:param goal_sample_prob: probability to sample goal position
:param position_tolerance: tolerance to goal position
:param duration: duration of motion
:return: path found or raise error
"""
util = self.util
cspace = self.cspace
rrt = RRT(TreeNode(self.q_initial), cspace)
q_goals = []
for i in range(max_iters):
print("iter: ", i)
sample = self.sample_config(util, goal_sample_prob)
neighbor = rrt.nearest(sample)
path = self.safe_path(cspace, neighbor.value, sample, util)
if len(path) > 1:
q_end = path[-1]
# If pure RRT, include intermediate points as nodes
q_new_node = neighbor
add_middle_nodes = not star
if add_middle_nodes:
middle_path = path[1:-1:10]
else:
middle_path = []
for q in middle_path + [q_end]:
if not star:
q_new_node = self.rrt_extend(rrt, q_new_node, q)
else:
q_new_node = self.rrt_star_extend(rrt, q_new_node, q)
# check for goal
translation = util.FK(q_end).translation()
if self.within_tol(translation, self.p_goal, position_tolerance):
q_goals.append(q_new_node)
if ((not star and len(q_goals) > 0) or
(star and len(q_goals) > 0 and i == max_iters-1)):
min_cost = None
best_path = None
for q_goal_node in q_goals:
path = np.array(rrt.bfs(q_goal_node.value))
cost = rrt.cost(q_goal_node)
if min_cost is None or cost < min_cost:
min_cost = cost
best_path = path
num_knot_points = best_path.shape[0]
t_knots = np.linspace(0, duration, num_knot_points)
qtraj = PiecewisePolynomial.Cubic(t_knots, best_path.T, np.zeros(7), np.zeros(7))
print("Path cost: {}".format(min_cost))
print("Total number of rewires: {}".format(self.rewires))
return [JointSpacePlan(qtraj)], [0.1]
raise Exception("Did not find path to goal")
def test_lagrange_interpolating_polynomial(self):
t = [0., 1., 2.]
x = np.diag([4., 5., 6.])
pp = PiecewisePolynomial.LagrangeInterpolatingPolynomial(times=t,
samples=x)
self.assertEqual(pp.get_number_of_segments(), 1)
np.testing.assert_array_almost_equal(x[:, [1]], pp.value(1.), 1e-12)
def polynomial(self):
d, n = self.q_knots.shape
return PiecewisePolynomial.Cubic(
breaks=self.t_knots,
knots=self.q_knots,
knot_dot_start=np.zeros(d),
knot_dot_end=np.zeros(d))
def test_direct_collocation(self):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.2,
maximum_timestep=0.5)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(0)
x = dircol.state()
x2 = dircol.state(2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(0.3, 0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(.3 * 21, 0.4 * 21)
dircol.AddFinalCost(2 * x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3 * 21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(initial_u, initial_x)
dircol.Solve()
times = dircol.GetSampleTimes()
inputs = dircol.GetInputSamples()
states = dircol.GetStateSamples()
input_traj = dircol.ReconstructInputTrajectory()
state_traj = dircol.ReconstructStateTrajectory()
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
def buildTrajectorySource(ts, knots):
#good_steps = np.diff(np.hstack([ts_raw, np.array(ts_raw[-1])])) >= 0.001
#ts = ts_raw[good_steps]
#knots = knots_raw[:, good_steps]
ppt = PiecewisePolynomial.FirstOrderHold(ts, knots)
return TrajectorySource(trajectory=ppt,
output_derivative_order=0,
zero_derivatives_beyond_limits=True)
def test_first_order_hold(self):
x = np.array([[1., 2.], [3., 4.], [5., 6.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
np.testing.assert_equal(np.array([[2.], [3.]]), pp.value(.5))
deriv = pp.MakeDerivative(derivative_order=1)
np.testing.assert_equal(np.array([[2.], [2.]]), deriv.value(.5))
pp.AppendFirstOrderSegment(time=3., sample=[-0.4, .57])
def _do_test_direct_transcription(self, use_deprecated_solve):
# Integrator.
plant = LinearSystem(A=[0.], B=[1.], C=[1.], D=[0.], time_period=0.1)
context = plant.CreateDefaultContext()
dirtran = DirectTranscription(plant, context, num_time_samples=21)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dirtran.time()
dt = dirtran.fixed_timestep()
x = dirtran.state()
x2 = dirtran.state(2)
x0 = dirtran.initial_state()
xf = dirtran.final_state()
u = dirtran.input()
u2 = dirtran.input(2)
dirtran.AddRunningCost(x.dot(x))
dirtran.AddConstraintToAllKnotPoints(u[0] == 0)
dirtran.AddFinalCost(2*x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dirtran.SetInitialTrajectory(initial_u, initial_x)
if use_deprecated_solve:
with warnings.catch_warnings(record=True) as w:
warnings.simplefilter('always', DrakeDeprecationWarning)
dirtran.Solve()
times = dirtran.GetSampleTimes()
inputs = dirtran.GetInputSamples()
states = dirtran.GetStateSamples()
input_traj = dirtran.ReconstructInputTrajectory()
state_traj = dirtran.ReconstructStateTrajectory()
self.assertEqual(len(w), 6)
else:
result = mp.Solve(dirtran)
times = dirtran.GetSampleTimes(result)
inputs = dirtran.GetInputSamples(result)
states = dirtran.GetStateSamples(result)
input_traj = dirtran.ReconstructInputTrajectory(result)
state_traj = dirtran.ReconstructStateTrajectory(result)
def GetCurrentPlan(self, context):
if not self.kuka_plans_list:
return
t = context.get_time()
if self.kuka_plans_list[0].traj is None:
q_current = self.EvalVectorInput(
context,
self.iiwa_position_input_port.get_index()).get_value()
q0 = self.kuka_plans_list[2].traj.value(0).flatten()
# zero order hold
q_knots_kuka = np.zeros((2, 7))
q_knots_kuka[0] = q_current
qtraj = PiecewisePolynomial.ZeroOrderHold(
[0, self.zero_order_hold_duration_sec], q_knots_kuka.T)
self.kuka_plans_list[0] = JointSpacePlan(qtraj)
# move to the starting position of trajectory plans
t_knots = np.array([
0., self.move_to_home_duration_sec / 2,
self.move_to_home_duration_sec
])
q_knots_kuka = np.zeros((3, 7))
q_knots_kuka[0] = q_current
q_knots_kuka[2] = q0
q_knots_kuka[1] = (q_knots_kuka[0] + q_knots_kuka[2]) / 2
qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T,
np.zeros(7), np.zeros(7))
self.kuka_plans_list[1] = JointSpacePlan(qtraj)
# set current plan
self.current_plan_idx = 0
self.current_plan = self.kuka_plans_list[0]
self.current_plan.set_start_time(0.)
new_plan_idx = 0
for i in range(self.num_plans):
if t >= self.t_plan[i] and t < self.t_plan[i + 1]:
new_plan_idx = i
break
if self.current_plan_idx < new_plan_idx:
self.current_plan_idx = new_plan_idx
self.current_plan = self.kuka_plans_list[new_plan_idx]
self.current_plan.set_start_time(t)
def test_reverse_and_scale_time(self):
x = np.array([[10.], [20.], [30.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0.5, 1., 2.], x)
pp.ReverseTime()
self.assertEqual(pp.start_time(), -2.0)
self.assertEqual(pp.end_time(), -0.5)
pp.ScaleTime(2.0)
self.assertEqual(pp.start_time(), -4.0)
self.assertEqual(pp.end_time(), -1.0)
def ConnectPointsWithCubicPolynomial(x_start, x_end, duration):
t_knots = [0, duration / 2, duration]
n = len(x_start)
assert n == len(x_end)
x_knots = np.zeros((3, n))
x_knots[0] = x_start
x_knots[2] = x_end
x_knots[1] = (x_knots[0] + x_knots[2]) / 2
return PiecewisePolynomial.Cubic(t_knots, x_knots.T, np.zeros(n),
np.zeros(n))
def test_zero_order_hold_matrix(self):
A0 = np.array([[1, 2, 3], [4, 5, 6]])
A1 = np.array([[7, 8, 9], [10, 11, 12]])
A2 = np.array([[13, 14, 15], [16, 17, 18]])
pp = PiecewisePolynomial.ZeroOrderHold([0., 1., 2.], [A0, A1, A2])
self.assertEqual(pp.get_number_of_segments(), 2)
self.assertEqual(pp.start_time(), 0.)
self.assertEqual(pp.end_time(), 2.)
self.assertEqual(pp.rows(), 2)
self.assertEqual(pp.cols(), 3)
np.testing.assert_equal(A0, pp.value(.5))
def make_finger_trajectory(times):
opened = np.array([0.08])
closed = np.array([0.00])
traj_hand_command = PiecewisePolynomial.FirstOrderHold(
[times["initial"], times["pick_start"]], np.hstack([[opened], [opened]]))
traj_hand_command.AppendFirstOrderSegment(times["pick_end"], closed)
traj_hand_command.AppendFirstOrderSegment(times["place_start"], closed)
traj_hand_command.AppendFirstOrderSegment(times["place_end"], opened)
traj_hand_command.AppendFirstOrderSegment(times["postplace"], opened)
return traj_hand_command
def test_slice_and_shift(self):
x = np.array([[10.], [20.], [30.]]).transpose()
pp = PiecewisePolynomial.FirstOrderHold([0., 1., 2.], x)
pp_sub = pp.slice(start_segment_index=1, num_segments=1)
self.assertEqual(pp_sub.get_number_of_segments(), 1)
self.assertEqual(pp_sub.start_time(), 1.)
self.assertEqual(pp_sub.end_time(), 2.)
values_sub = np.array(list(map(pp_sub.value, [1., 2.])))
self.assertTrue((values_sub == [[[20.]], [[30.]]]).all())
pp_sub.shiftRight(10.)
self.assertEqual(pp_sub.start_time(), 11.)
self.assertEqual(pp_sub.end_time(), 12.)
def test_reshape_and_block(self):
t = [0., 1., 2., 3.]
x = np.diag([4., 5., 6., 7.])
pp = PiecewisePolynomial.FirstOrderHold(t, x)
self.assertEqual(pp.rows(), 4)
self.assertEqual(pp.cols(), 1)
pp.Reshape(rows=2, cols=2)
self.assertEqual(pp.rows(), 2)
self.assertEqual(pp.cols(), 2)
pp2 = pp.Block(start_row=0, start_col=0, block_rows=2, block_cols=1)
self.assertEqual(pp2.rows(), 2)
self.assertEqual(pp2.cols(), 1)
def test_zero_order_hold(self):
x = np.array([[1., 2.], [3., 4.], [5., 6.]]).transpose()
pp = PiecewisePolynomial.ZeroOrderHold([0., 1., 2.], x)
np.testing.assert_equal(np.array([[1.], [2.]]), pp.value(.5))
self.assertEqual(pp.get_number_of_segments(), 2)
self.assertEqual(pp.start_time(), 0.)
self.assertEqual(pp.end_time(), 2.)
self.assertEqual(pp.start_time(0), 0.)
self.assertEqual(pp.end_time(0), 1.)
self.assertEqual(pp.duration(0), 1.)
self.assertEqual(pp.get_segment_index(1.5), 1)
self.assertEqual(pp.get_segment_times(), [0., 1., 2.])
def __init__(self,
angle_start,
angle_end=np.pi / 4,
duration=10.0,
type=None):
t_knots = [0, duration / 2, duration]
door_angle_knots = np.zeros((3, 1))
door_angle_knots[0] = angle_start
door_angle_knots[2] = angle_end
door_angle_knots[1] = (door_angle_knots[0] + door_angle_knots[2]) / 2
angle_traj = PiecewisePolynomial.Cubic(t_knots, door_angle_knots.T,
np.zeros(1), np.zeros(1))
PlanBase.__init__(self, type=type, trajectory=angle_traj)
def reconstruct_path(self, node):
"""
Piece together trajectories between nodes in the path to selected node (generally self.best_goal_node).
"""
current = node
utraj, xtraj, ttraj, res, cost = self.plant.trajOptRRT(
current.parent.state,
current.state,
goal=current.goal_node,
verbose=False)
urrt = utraj
xrrt = xtraj
trrt = ttraj
current = current.parent
while current.parent is not None:
utraj, xtraj, ttraj, res, cost = self.plant.trajOptRRT(
current.parent.state,
current.state,
goal=current.goal_node,
verbose=False)
urrt = np.vstack((utraj, urrt))
xrrt = np.vstack((xtraj[:-1, :], xrrt))
trrt = np.hstack((ttraj[:-1], trrt + ttraj.max()))
current = current.parent
# save traj
self.plant.udtraj = urrt
self.plant.xdtraj = xrrt
self.plant.ttraj = trrt
self.plant.mp_result = res
self.plant.udtraj_poly = PiecewisePolynomial.FirstOrderHold(
trrt[0:-1], urrt.T)
self.plant.xdtraj_poly = PiecewisePolynomial.Cubic(trrt, xrrt.T)
return urrt, xrrt, trrt
def convert_splines(mbp, robot, gripper, context, trajectories):
# TODO: move to trajectory class
print()
splines, gripper_setpoints = [], []
for i, traj in enumerate(trajectories):
traj.path[-1].assign(context)
joints = traj.path[0].joints
if len(joints) == 8: # TODO: fix this
joints = joints[:7]
if len(joints) == 2:
q_knots_kuka = np.zeros((2, 7))
q_knots_kuka[0] = get_configuration(mbp, context, robot) # Second is velocity
splines.append(PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots_kuka.T))
elif len(joints) == 7:
# TODO: adjust timing based on distance & velocities
# TODO: adjust number of waypoints
distance_fn = get_distance_fn(joints)
#path = [traj.path[0].positions, traj.path[-1].positions]
path = [q.positions[:len(joints)] for q in traj.path]
path = waypoints_from_path(joints, path) # TODO: increase time for pick/place & hold
q_knots_kuka = np.vstack(path).T
distances = [0.] + [distance_fn(q1, q2) for q1, q2 in zip(path, path[1:])]
t_knots = np.cumsum(distances) / RADIANS_PER_SECOND # TODO: this should be a max
d, n = q_knots_kuka.shape
print('{}) d={}, n={}, duration={:.3f}'.format(i, d, n, t_knots[-1]))
splines.append(PiecewisePolynomial.Cubic(
breaks=t_knots,
knots=q_knots_kuka,
knot_dot_start=np.zeros(d),
knot_dot_end=np.zeros(d)))
# RuntimeError: times must be in increasing order.
else:
raise ValueError(joints)
_, gripper_setpoint = get_configuration(mbp, context, gripper)
gripper_setpoints.append(gripper_setpoint)
return splines, gripper_setpoints
def test_piecewise_polynomial_constant_constructor(self):
x = np.array([[1.], [4.]])
pp = PiecewisePolynomial(x)
self.assertEqual(pp.rows(), 2)
self.assertEqual(pp.cols(), 1)
np.testing.assert_equal(x, pp.value(11.))
