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