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 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 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_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 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 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 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 _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 test_zero_order_hold(self):
x = np.array([[1., 2.], [3., 4.], [5., 6.]]).transpose()
pp = PiecewisePolynomial.ZeroOrderHold([0., 1., 2.], x)
pp_d = pp.derivative(derivative_order=1)
np.testing.assert_equal(np.array([[1.], [2.]]), pp.value(.5))
np.testing.assert_equal(pp_d.value(.5), np.array([[0.], [0.]]))
p = pp.getPolynomial(segment_index=0, row=1, col=0)
np.testing.assert_equal(p.GetCoefficients(), np.array([2]))
self.assertEqual(pp.getSegmentPolynomialDegree(segment_index=1), 0)
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(segment_index=0), 0.)
self.assertEqual(pp.end_time(segment_index=0), 1.)
self.assertEqual(pp.duration(segment_index=0), 1.)
self.assertTrue(pp.is_time_in_range(t=1.5))
self.assertEqual(pp.get_segment_index(t=1.5), 1)
self.assertEqual(pp.get_segment_times(), [0., 1., 2.])
def main():
# Default filename for the example
filename = FindResourceOrThrow(
"examples/Cassie/saved_trajectories/jumping_0.2h_0.25d")
if len(sys.argv) == 2:
filename = sys.argv[1]
dircon_traj = pydairlib.lcm_trajectory.DirconTrajectory(filename)
# Reconstructing state and input trajectory as piecewise polynomials
state_traj = dircon_traj.ReconstructStateTrajectory()
input_traj = dircon_traj.ReconstructInputTrajectory()
state_datatypes = dircon_traj.GetTrajectory("state_traj0").datatypes
input_datatypes = dircon_traj.GetTrajectory("input_traj").datatypes
force_traj = PiecewisePolynomial.ZeroOrderHold(
dircon_traj.GetForceBreaks(0), dircon_traj.GetForceSamples(0))
force_datatypes = dircon_traj.GetTrajectory("force_vars0").datatypes
collocation_force_points = dircon_traj.GetCollocationForceSamples(0)
n_points = 500
t = np.linspace(state_traj.start_time(), state_traj.end_time(), n_points)
state_samples = np.zeros((n_points, state_traj.value(0).shape[0]))
input_samples = np.zeros((n_points, input_traj.value(0).shape[0]))
force_samples = np.zeros((n_points, force_traj.value(0).shape[0]))
for i in range(n_points):
state_samples[i] = state_traj.value(t[i])[:, 0]
input_samples[i] = input_traj.value(t[i])[:, 0]
force_samples[i] = force_traj.value(t[i])[:, 0]
# Plotting reconstructed state trajectories
plt.figure("state trajectory")
plt.plot(t, state_samples)
plt.legend(state_datatypes)
plt.figure("input trajectory")
plt.plot(t, input_samples)
plt.legend(input_datatypes)
plt.figure("force trajectory")
plt.plot(t, force_samples)
plt.legend(force_datatypes)
plt.show()
def test_matrix_trajectories(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]])
t = [0., 1., 2.]
pp = dict()
pp["zoh"] = PiecewisePolynomial.ZeroOrderHold(breaks=t,
samples=[A0, A1, A2])
pp["foh"] = PiecewisePolynomial.FirstOrderHold(breaks=t,
samples=[A0, A1, A2])
pp["hermite"] = PiecewisePolynomial.CubicShapePreserving(
breaks=t, samples=[A0, A1, A2], zero_end_point_derivatives=False)
pp["c1"] = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t, samples=[A0, A1, A2], periodic_end=False)
pp["c2"] = PiecewisePolynomial.CubicHermite(
breaks=t,
samples=[A0, A1, A2],
samples_dot=[0 * A0, 0 * A1, 0 * A2])
pp["c3"] = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
breaks=t,
samples=[A0, A1, A2],
sample_dot_at_start=0 * A0,
sample_dot_at_end=0 * A0)
pp["lagrange"] = PiecewisePolynomial.LagrangeInterpolatingPolynomial(
times=t, samples=[A0, A1, A2])
for name, traj in pp.items():
if name == "lagrange":
self.assertEqual(traj.get_number_of_segments(), 1)
else:
self.assertEqual(traj.get_number_of_segments(), 2)
self.assertEqual(traj.start_time(), 0.)
self.assertEqual(traj.end_time(), 2.)
self.assertEqual(traj.rows(), 2)
self.assertEqual(traj.cols(), 3)
# Check the values for the easy cases:
np.testing.assert_equal(A0, pp["zoh"].value(.5))
np.testing.assert_allclose(0.5 * A0 + 0.5 * A1, pp["foh"].value(.5),
1e-15)
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_direct_collocation(self):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
num_time_samples = 21
dircol = DirectCollocation(
plant,
context,
num_time_samples=num_time_samples,
minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
prog = dircol.prog()
# 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(index=0)
x = dircol.state()
x2 = dircol.state(index=2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(index=2)
v = dircol.NewSequentialVariable(rows=1, name="test")
v2 = dircol.GetSequentialVariableAtIndex(name="test", index=2)
dircol.AddRunningCost(x.dot(x))
input_con = dircol.AddConstraintToAllKnotPoints(u[0] == 0)
self.assertEqual(len(input_con), 21)
interval_bound = dircol.AddTimeIntervalBounds(lower_bound=0.3,
upper_bound=0.4)
self.assertIsInstance(interval_bound.evaluator(),
mp.BoundingBoxConstraint)
equal_time_con = dircol.AddEqualTimeIntervalsConstraints()
self.assertEqual(len(equal_time_con), 19)
duration_bound = dircol.AddDurationBounds(lower_bound=.3 * 21,
upper_bound=0.4 * 21)
self.assertIsInstance(duration_bound.evaluator(), mp.LinearConstraint)
final_cost = dircol.AddFinalCost(2 * x.dot(x))
self.assertIsInstance(final_cost.evaluator(), mp.Cost)
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3 * 21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(traj_init_u=initial_u,
traj_init_x=initial_x)
was_called = dict(input=False, state=False, complete=False)
def input_callback(t, u):
was_called["input"] = True
def state_callback(t, x):
was_called["state"] = True
def complete_callback(t, x, u, v):
was_called["complete"] = True
dircol.AddInputTrajectoryCallback(callback=input_callback)
dircol.AddStateTrajectoryCallback(callback=state_callback)
dircol.AddCompleteTrajectoryCallback(callback=complete_callback,
names=["test"])
result = mp.Solve(dircol.prog())
self.assertTrue(was_called["input"])
self.assertTrue(was_called["state"])
self.assertTrue(was_called["complete"])
dircol.GetSampleTimes(result=result)
dircol.GetInputSamples(result=result)
dircol.GetStateSamples(result=result)
dircol.GetSequentialVariableSamples(result=result, name="test")
dircol.ReconstructInputTrajectory(result=result)
dircol.ReconstructStateTrajectory(result=result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1), prog)
# Test AddConstraintToAllKnotPoints variants.
nc = len(prog.bounding_box_constraints())
c = dircol.AddConstraintToAllKnotPoints(
constraint=mp.BoundingBoxConstraint([0], [1]), vars=u)
self.assertIsInstance(c[0], mp.Binding[mp.BoundingBoxConstraint])
self.assertEqual(len(prog.bounding_box_constraints()),
nc + num_time_samples)
nc = len(prog.linear_equality_constraints())
c = dircol.AddConstraintToAllKnotPoints(
constraint=mp.LinearEqualityConstraint([1], [0]), vars=u)
self.assertIsInstance(c[0], mp.Binding[mp.LinearEqualityConstraint])
self.assertEqual(len(prog.linear_equality_constraints()),
nc + num_time_samples)
nc = len(prog.linear_constraints())
c = dircol.AddConstraintToAllKnotPoints(constraint=mp.LinearConstraint(
[1], [0], [1]),
vars=u)
self.assertIsInstance(c[0], mp.Binding[mp.LinearConstraint])
self.assertEqual(len(prog.linear_constraints()), nc + num_time_samples)
nc = len(prog.linear_equality_constraints())
# eq(x, 2) produces a 2-dimensional vector of Formula.
c = dircol.AddConstraintToAllKnotPoints(eq(x, 2))
self.assertIsInstance(c[0].evaluator(), mp.LinearEqualityConstraint)
self.assertEqual(len(prog.linear_equality_constraints()),
nc + 2 * num_time_samples)
def polynomial(self):
q_knots_kuka = np.zeros((2, len(self.q_knot)))
q_knots_kuka[0] = self.q_knot
return PiecewisePolynomial.ZeroOrderHold([0, self.duration], q_knots_kuka.T)
def get_hold_spline(mbp, context, robot, duration=2.0):
q_knots_kuka = np.zeros((2, 7))
q_knots_kuka[0] = get_configuration(mbp, context,
robot) # Second is velocity
return PiecewisePolynomial.ZeroOrderHold([0, duration], q_knots_kuka.T)
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,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
# 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(index=0)
x = dircol.state()
x2 = dircol.state(index=2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(index=2)
v = dircol.NewSequentialVariable(rows=1, name="test")
v2 = dircol.GetSequentialVariableAtIndex(name="test", index=2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(lower_bound=0.3, upper_bound=0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(lower_bound=.3*21, upper_bound=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(traj_init_u=initial_u,
traj_init_x=initial_x)
was_called = dict(
input=False,
state=False,
complete=False
)
def input_callback(t, u):
was_called["input"] = True
def state_callback(t, x):
was_called["state"] = True
def complete_callback(t, x, u, v):
was_called["complete"] = True
dircol.AddInputTrajectoryCallback(callback=input_callback)
dircol.AddStateTrajectoryCallback(callback=state_callback)
dircol.AddCompleteTrajectoryCallback(callback=complete_callback,
names=["test"])
result = mp.Solve(dircol)
self.assertTrue(was_called["input"])
self.assertTrue(was_called["state"])
self.assertTrue(was_called["complete"])
times = dircol.GetSampleTimes(result=result)
inputs = dircol.GetInputSamples(result=result)
states = dircol.GetStateSamples(result=result)
variables = dircol.GetSequentialVariableSamples(result=result,
name="test")
input_traj = dircol.ReconstructInputTrajectory(result=result)
state_traj = dircol.ReconstructStateTrajectory(result=result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
def GenerateIiwaTrajectoriesAndGripperSetPoints(X_WO, is_printing=True):
"""
:param X_WO: the pose of the foam brick in world frame.
:param is_printing: set to True to print IK solution results.
:return: 1. a list of joint space trajectories.
2. a list of gripper set points.
The two lists must have the same length.
"""
station = ManipulationStation()
station.SetupManipulationClassStation()
station.Finalize()
plant = station.get_controller_plant()
# Go to p_WQ_home
ik_iiwa = inverse_kinematics.InverseKinematics(plant)
world_frame = plant.world_frame()
l7_frame = plant.GetFrameByName("iiwa_link_7")
theta_bound = 0.005 * np.pi
R_WL7 = RollPitchYaw(0, np.pi * 5 / 6, 0).ToRotationMatrix()
ik_iiwa.AddOrientationConstraint(
frameAbar=world_frame, R_AbarA=R_WL7, # rotate end effector to R_WL7 angle
frameBbar=l7_frame, R_BbarB=RotationMatrix(),
theta_bound=theta_bound)
ik_iiwa.AddPositionConstraint(
frameB=l7_frame, p_BQ=p_L7Q,
frameA=world_frame,
p_AQ_lower=p_WQ_home - 0.01, #p_WQ_home corresponds to q_val_0's pos
p_AQ_upper=p_WQ_home + 0.01)
prog = ik_iiwa.prog()
prog.SetInitialGuess(ik_iiwa.q(), np.zeros(7))
result = mp.Solve(prog)
if is_printing:
print(result.get_solution_result())
q_val_0 = result.GetSolution(ik_iiwa.q())
qtraj_leave_home = ConnectPointsWithCubicPolynomial(q_home, q_val_0, 2.0)
# close fingers
q_knots = np.zeros((2, 7))
q_knots[0] = qtraj_leave_home.value(qtraj_leave_home.end_time()).squeeze()
q_knots[1] = q_knots[0]
qtraj_open_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)
# Complete your pick and place trajectories.
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.
q_curr = q_knots[1]
p_mid = np.array([0.53, 0.02, 0.38])
a_box = 0.14
tilt_link_7 = lambda i, n: RollPitchYaw(a_box*i/(n-1), 5/6*np.pi, 0).ToRotationMatrix()
RPY = RollPitchYaw(a_box, np.pi * 5 / 6, 0).ToRotationMatrix()
qtraj_down = MoveEndEffectorAlongStraightLine(p_WQ_home, p_mid, 3, tilt_link_7, q_curr, 10)
q_curr = qtraj_down.value(qtraj_down.end_time()).squeeze()
p_box = np.array([0.52, 0.033, 0.18])
qtraj_down2 = MoveEndEffectorAlongStraightLine(p_mid, p_box, 4, lambda i, n: RPY, q_curr, 10)
q_curr = qtraj_down2.value(qtraj_down2.end_time()).squeeze()
p_up = np.array([0.53, 0.033, 0.38])
qtraj_up = MoveEndEffectorAlongStraightLine(p_box, p_up, 4, lambda i, n: RPY, q_curr, 10)
q_curr = qtraj_up.value(qtraj_up.end_time()).squeeze()
p_down = np.array([0.52, 0.033, 0.21])
qtraj_backdown = MoveEndEffectorAlongStraightLine(p_up, p_down, 4, lambda i, n: RPY, q_curr, 10)
q_curr = qtraj_backdown.value(qtraj_backdown.end_time()).squeeze()
qtraj_return = ConnectPointsWithCubicPolynomial(q_curr, q_home, 3.0)
q_traj_list = [qtraj_leave_home,
qtraj_open_gripper,
qtraj_down,
qtraj_down2,
qtraj_up,
qtraj_backdown,
qtraj_return
]
gripper_setpoint_list = [0.01, 0.1, 0.1, 0.1, 0.029, 0.029, 0.1]
return q_traj_list, gripper_setpoint_list
def _do_test_direct_collocation(self, use_deprecated_solve):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(
plant, context, num_time_samples=21, minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
# 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)
global input_was_called
input_was_called = False
global state_was_called
state_was_called = False
def input_callback(t, u):
global input_was_called
input_was_called = True
def state_callback(t, x):
global state_was_called
state_was_called = True
dircol.AddInputTrajectoryCallback(input_callback)
dircol.AddStateTrajectoryCallback(state_callback)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=1):
dircol.Solve()
result = None
else:
result = mp.Solve(dircol)
self.assertTrue(input_was_called)
self.assertTrue(state_was_called)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=5):
times = dircol.GetSampleTimes()
inputs = dircol.GetInputSamples()
states = dircol.GetStateSamples()
input_traj = dircol.ReconstructInputTrajectory()
state_traj = dircol.ReconstructStateTrajectory()
else:
times = dircol.GetSampleTimes(result)
inputs = dircol.GetInputSamples(result)
states = dircol.GetStateSamples(result)
input_traj = dircol.ReconstructInputTrajectory(result)
state_traj = dircol.ReconstructStateTrajectory(result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
def GenerateIiwaPlansAndGripperSetPoints(manip_station_sim, is_printing=True):
plant = manip_station_sim.plant
tree = plant.tree()
iiwa_model = plant.GetModelInstanceByName("iiwa")
gripper_model = plant.GetModelInstanceByName("gripper")
# get "home" pose
ik_scene = inverse_kinematics.InverseKinematics(plant)
world_frame = plant.world_frame()
gripper_frame = plant.GetFrameByName("body", gripper_model)
theta_bound = 0.005 * np.pi # 0.9 degrees
X_EEa = GetEndEffectorWorldAlignedFrame()
R_EEa = RotationMatrix(X_EEa.rotation())
ik_scene.AddOrientationConstraint(frameAbar=world_frame,
R_AbarA=R_WEa_ref,
frameBbar=gripper_frame,
R_BbarB=R_EEa,
theta_bound=theta_bound)
p_WQ0 = p_WQ_home
p_WQ_lower = p_WQ0 - 0.005
p_WQ_upper = p_WQ0 + 0.005
ik_scene.AddPositionConstraint(frameB=gripper_frame,
p_BQ=p_EQ,
frameA=world_frame,
p_AQ_lower=p_WQ_lower,
p_AQ_upper=p_WQ_upper)
prog = ik_scene.prog()
prog.SetInitialGuess(ik_scene.q(), np.zeros(plant.num_positions()))
result = prog.Solve()
if is_printing:
print result
q_val_0 = prog.GetSolution(ik_scene.q())
# q returned by IK consists of the configuration of all bodies, including
# the iiwa arm, the box, the gripper and the bottle.
# But the trajectory sent to iiwa only needs the configuration of iiwa.
# This function takes in an array of shape (n, plant.num_positions()),
# and returns an array of shape (n, 7), which only has the configuration of the iiwa arm.
def GetKukaQKnots(q_knots):
if len(q_knots.shape) == 1:
q_knots.resize(1, q_knots.size)
n = q_knots.shape[0]
q_knots_kuka = np.zeros((n, 7))
for i, q_knot in enumerate(q_knots):
q_knots_kuka[i] = tree.get_positions_from_array(iiwa_model, q_knot)
return q_knots_kuka
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
# inverse_kin_ponitwise
def GoFromPointToPoint(p_WQ_start,
p_WQ_end,
duration,
num_knot_points,
q_initial_guess,
InterpolatePosition=InterpolateStraightLine,
position_tolerance=0.005):
# The first knot point is the zero posture.
# The second knot is the pre-pre-grasp posture q_val_0
# The rest are solved for in the for loop below.
# The hope is that using more knot points makes the trajectory
# smoother.
q_knots = np.zeros((num_knot_points + 1, plant.num_positions()))
q_knots[0] = q_initial_guess
for i in range(num_knot_points):
ik = inverse_kinematics.InverseKinematics(plant)
q_variables = ik.q()
# ik.AddOrientationConstraint(
# frameAbar=world_frame, R_AbarA=R_WEa_ref,
# frameBbar=gripper_frame, R_BbarB=R_EEa,
# theta_bound=theta_bound)
p_WQ = InterpolatePosition(p_WQ_start, p_WQ_end, num_knot_points,
i)
ik.AddPositionConstraint(frameB=gripper_frame,
p_BQ=p_EQ,
frameA=world_frame,
p_AQ_lower=p_WQ - position_tolerance,
p_AQ_upper=p_WQ + position_tolerance)
prog = ik.prog()
# use the robot posture at the previous knot point as
# an initial guess.
prog.SetInitialGuess(q_variables, q_knots[i])
result = prog.Solve()
if is_printing:
print i, ": ", result
q_knots[i + 1] = prog.GetSolution(q_variables)
t_knots = np.linspace(0, duration, num_knot_points + 1)
q_knots_kuka = GetKukaQKnots(q_knots)
qtraj = PiecewisePolynomial.Cubic(t_knots, q_knots_kuka.T, np.zeros(7),
np.zeros(7))
return qtraj, q_knots
# Generating trajectories
num_knot_points = 10
# move to grasp left door handle
p_WQ_start = p_WQ0
p_WQ_end = p_WC_handle
qtraj_move_to_handle, q_knots_full = GoFromPointToPoint(
p_WQ_start,
p_WQ_end,
duration=5.0,
num_knot_points=num_knot_points,
q_initial_guess=q_val_0,
position_tolerance=0.001)
# close gripper
q_knots = np.zeros((2, 7))
q_knots[0] = qtraj_move_to_handle.value(
qtraj_move_to_handle.end_time()).squeeze()
qtraj_close_gripper = PiecewisePolynomial.ZeroOrderHold([0, 1], q_knots.T)
# pull handle along an arc
def InterpolateArc(angle_start, angle_end, num_knot_points, i):
radius = r_handle
theta = angle_start + (angle_end - angle_start) * (i +
1) / num_knot_points
return p_WC_left_hinge + [
-radius * np.sin(theta), -radius * np.cos(theta), 0
]
angle_start = theta0_hinge
angle_end = np.pi / 180 * 60
qtraj_pull_handle, q_knots_full = GoFromPointToPoint(
angle_start,
angle_end,
duration=5.0,
num_knot_points=20,
q_initial_guess=q_knots_full[-1],
InterpolatePosition=InterpolateArc,
position_tolerance=0.002)
q_traj_list = [
qtraj_move_to_handle, qtraj_close_gripper, qtraj_pull_handle
]
plan_list = []
for q_traj in q_traj_list:
plan_list.append(JointSpacePlan(q_traj))
gripper_setpoint_list = [0.03, 0.0, 0.0]
return plan_list, gripper_setpoint_list
