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.))