def runDircol(self,x0,xf,tf0): N = 15 # constant #N = np.int(tf0 * 10) # "10Hz" / samples per second context = self.CreateDefaultContext() dircol = DirectCollocation(self, context, num_time_samples=N, minimum_timestep=0.05, maximum_timestep=1.0) u = dircol.input() # set some constraints on inputs dircol.AddEqualTimeIntervalsConstraints() dircol.AddConstraintToAllKnotPoints(u[1] <= self.slewmax) dircol.AddConstraintToAllKnotPoints(u[1] >= -self.slewmax) dircol.AddConstraintToAllKnotPoints(u[0] <= self.umax) dircol.AddConstraintToAllKnotPoints(u[0] >= -self.umax) # constrain the last input to be zero (at least for the u input) #import pdb; pdb.set_trace() dv = dircol.decision_variables() for i in range(3, self.nX*N, 4): alfa_state = dv[i] #u[t_end] dircol.AddBoundingBoxConstraint(-self.alfamax, self.alfamax, alfa_state) #final_u_decision_var = dv[self.nX*N + self.nU*N - 1] #u[t_end] #dircol.AddLinearEqualityConstraint(final_u_decision_var, 0.0) #first_u_decision_var = dv[self.nX*N + 1 ] #u[t_0] #dircol.AddLinearEqualityConstraint(first_u_decision_var, 0.0) # set some constraints on start and final pose eps = 0.0 * np.ones(self.nX) # relaxing factor dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state()) dircol.AddBoundingBoxConstraint(xf-eps, \ xf+eps, dircol.final_state()) R = 1.0*np.eye(self.nU) # Cost on input "effort". dircol.AddRunningCost( u.transpose().dot(R.dot(u)) ) # Add a final cost equal to the total duration. dircol.AddFinalCost(dircol.time()) # guess initial trajectory initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) # optimize result = Solve(dircol) print('******\nRunning trajectory optimization:') print('w/ solver %s' %(result.get_solver_id().name())) print(result.get_solution_result()) assert(result.is_success()) xtraj = dircol.ReconstructStateTrajectory(result) utraj = dircol.ReconstructInputTrajectory(result) # return nominal trajectory return utraj,xtraj
def make_gripper_position_trajectory(X_G, times): """ Constructs a gripper position trajectory from the plan "sketch". """ # The syntax here is a little ugly; we need to pass in a matrix with the samples in the columns. # TODO(russt): Add python bindings for the std::vector constructor. traj = PiecewisePolynomial.FirstOrderHold( [times["initial"], times["prepick"]], np.vstack([X_G["initial"].translation(), X_G["prepick"].translation()]).T) # TODO(russt): I could make this less brittle if I was more careful on the names above, and just look up the pose for every time (in order) traj.AppendFirstOrderSegment(times["pick_start"], X_G["pick"].translation()) traj.AppendFirstOrderSegment(times["pick_end"], X_G["pick"].translation()) traj.AppendFirstOrderSegment(times["postpick"], X_G["prepick"].translation()) traj.AppendFirstOrderSegment(times["clearance"], X_G["clearance"].translation()) traj.AppendFirstOrderSegment(times["preplace"], X_G["preplace"].translation()) traj.AppendFirstOrderSegment(times["place_start"], X_G["place"].translation()) traj.AppendFirstOrderSegment(times["place_end"], X_G["place"].translation()) traj.AppendFirstOrderSegment(times["postplace"], X_G["preplace"].translation()) return traj
def _plan_hybrid_traj(self, initial_state, t, T, d): """ Returns a trajectory and associated cost (x_traj, total_cost) for a given hybrid mode. If c and t are None, then we are simply planning a contact-free trajectory. """ try: if t is None: print("Planning hybrid traj without final state constraint") traj_x, total_cost = self._solve_traj_opt(initial_state, False, (T, T), d) return traj_x, None, total_cost x_traj_nc, cost_nc = self._solve_traj_opt(initial_state, True, (t, t), d) # append zero control and x_traj_t..T constant at final_state if t >= T: x_traj_c = None else: # TODO fix this x_traj_c = PiecewisePolynomial.FirstOrderHold([t, T], np.column_stack((initial_state, initial_state))) cost_c = 0. # TODO compute this cost return x_traj_nc, x_traj_c, cost_nc + cost_c except RuntimeError as e: raise e
def test_trajectories(self): """construct_v_w_trajectories""" f = self.notebook_locals['construct_v_w_trajectories'] key_frames = self.notebook_locals['test_key_frames'] times = self.notebook_locals['times'] # Make the trajectories traj_v_G_test, traj_w_G_test = f(times, key_frames) self.assertTrue(isinstance(traj_v_G_test, PiecewisePolynomial)) self.assertTrue(isinstance(traj_w_G_test, PiecewisePolynomial)) # use student's answer on first problem to construct trajectories key_frame_pos = [] for kf in key_frames: key_frame_pos.append(kf.translation()) key_frame_pos = np.asarray(key_frame_pos) key_frame_ori = [pose.rotation() for pose in key_frames] traj_position = PiecewisePolynomial.FirstOrderHold( times, key_frame_pos.T) traj_rotation = PiecewiseQuaternionSlerp(times, key_frame_ori) traj_vG_true = traj_position.MakeDerivative() traj_wG_true = traj_rotation.MakeDerivative() self.assertTrue( traj_wG_true.isApprox(traj_w_G_test, tol=1e-3, tol_type=ToleranceType.absolute)) self.assertTrue( traj_vG_true.isApprox(traj_v_G_test, tol=1e-3, tol_type=ToleranceType.absolute))
def LQR(self, ztraj, utraj, Q, R, Qf): tspan = utraj.get_segment_times() dztrajdt = ztraj.derivative(1) context = self.CreateDefaultContext() nZ = context.num_continuous_states() nU = self.GetInputPort('u').size() sym_system = self.ToSymbolic() sym_context = sym_system.CreateDefaultContext() prog = MathematicalProgram() z = prog.NewIndeterminates(nZ,'z') ucon = prog.NewIndeterminates(nU,'u') #nY = self.GetOutputPort('z').size() #N = np.zeros([nX, nU]) times = ztraj.get_segment_times() K = [] S = [] for t in times: # option 1 z0 = ztraj.value(t).transpose()[0] u0 = utraj.value(t).transpose()[0] sym_context.SetContinuousState(z0+z) sym_context.FixInputPort(0, u0+ucon ) # zdot=f(z,u)==>zhdot=f(zh+z0,uh+u0)-z0dot f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector() # - dztrajdt.value(t).transpose() mapping = dict(zip(z, z0)) mapping.update(dict(zip(ucon, u0))) A = Evaluate(Jacobian(f, z), mapping) B = Evaluate(Jacobian(f, ucon), mapping) k, s = LinearQuadraticRegulator(A, B, Q, R) import pdb; pdb.set_trace() if(len(K) == 0): K = np.ravel(k).reshape(nU*nZ,1) S = np.ravel(s).reshape(nZ*nZ,1) else: K = np.hstack( (K, np.ravel(k).reshape(nU*nZ,1)) ) S = np.hstack( (S, np.ravel(s).reshape(nZ*nZ,1)) ) # # option 2 #context.SetContinuousState(xtraj.value(t) ) #context.FixInputPort(0, utraj.value(t) ) #linearized_plant = Linearize(self, context) #K.append(LinearQuadraticRegulator(linearized_plant.A(), # linearized_plant.B(), # Q, R)) #self, context, Q, R) Kpp = PiecewisePolynomial.FirstOrderHold(times, K) return Kpp
def runDircol(self, x0, xf, tf0): N = 21 #np.int(tf0 * 10) # "10Hz" samples per second context = self.CreateDefaultContext() dircol = DirectCollocation(self, context, num_time_samples=N, minimum_timestep=0.05, maximum_timestep=1.0) u = dircol.input() dircol.AddEqualTimeIntervalsConstraints() dircol.AddConstraintToAllKnotPoints(u[0] <= 0.5 * self.omegamax) dircol.AddConstraintToAllKnotPoints(u[0] >= -0.5 * self.omegamax) dircol.AddConstraintToAllKnotPoints(u[1] <= 0.5 * self.umax) dircol.AddConstraintToAllKnotPoints(u[1] >= -0.5 * self.umax) eps = 0.0 dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state()) dircol.AddBoundingBoxConstraint(xf - np.array([eps, eps, eps]), xf + np.array([eps, eps, eps]), dircol.final_state()) R = 1.0 * np.eye(2) # Cost on input "effort". dircol.AddRunningCost(u.transpose().dot(R.dot(u))) #dircol.AddRunningCost(R*u[0]**2) # Add a final cost equal to the total duration. dircol.AddFinalCost(dircol.time()) initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) result = Solve(dircol) print(result.get_solver_id().name()) print(result.get_solution_result()) assert (result.is_success()) #import pdb; pdb.set_trace() xtraj = dircol.ReconstructStateTrajectory(result) utraj = dircol.ReconstructInputTrajectory(result) return utraj, xtraj
def get_trajectory_data(self, ti): h_sol = self.prog.GetSolution(self.h[ti])[0] breaks = [h_sol*i for i in range(self.num_samples)] knots = self.prog.GetSolution(self.x[ti]) x_trajectory = PiecewisePolynomial.Cubic(breaks, knots, False) # t_samples = np.linspace(breaks[0], breaks[-1], 45) t_samples = np.linspace(breaks[0], breaks[-1], 100) x_samples = np.hstack([x_trajectory.value(t) for t in t_samples]) return x_trajectory, t_samples, x_samples
def make_wsg_command_trajectory(times): traj_wsg_command = PiecewisePolynomial.FirstOrderHold( [times["initial"], times["pick_start"]], np.hstack([[opened], [opened]])) traj_wsg_command.AppendFirstOrderSegment(times["pick_end"], closed) traj_wsg_command.AppendFirstOrderSegment(times["place_start"], closed) traj_wsg_command.AppendFirstOrderSegment(times["place_end"], opened) traj_wsg_command.AppendFirstOrderSegment(times["postplace"], opened) return traj_wsg_command
def make_real_dircol_mp(): global tree global plant global context global dircol # expmt = "acrobot" # State: (theta1, theta2, theta1_dot, theta2_dot) Input: Elbow torque tree = RigidBodyTree("/opt/underactuated/src/acrobot/acrobot.urdf", FloatingBaseType.kFixed) plant = AcrobotPlant() context = plant.CreateDefaultContext() dircol = DirectCollocation(plant, context, num_time_samples=21, minimum_timestep=0.05, maximum_timestep=0.2) dircol.AddEqualTimeIntervalsConstraints() # Add input limits. torque_limit = 8.0 # N*m. u = dircol.input() dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0]) dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit) initial_state = (0., 0., 0., 0.) dircol.AddBoundingBoxConstraint(initial_state, initial_state, dircol.initial_state()) final_state = (math.pi, 0., 0., 0.) dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state()) R = 10 # Cost on input "effort". u = dircol.input() dircol.AddRunningCost(R*u[0]**2) # Add a final cost equal to the total duration. dircol.AddFinalCost(dircol.time()) initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., 4.], np.column_stack((initial_state, final_state))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) print(id(dircol)) return dircol
def _check_trajectory(self, traj): if traj is None: plant_context = self.plant.CreateDefaultContext() pose = self.plant.GetPositions(plant_context) pose = np.column_stack((pose, pose)) pose = zero_pad_rows(pose, self.plant.num_positions() + self.plant.num_velocities()) return PiecewisePolynomial.FirstOrderHold([0., 1.], pose) elif type(traj) is np.ndarray: if traj.ndim == 1: traj = np.column_stack(traj, traj) traj = zero_pad_rows(traj, self.plant.num_positions() + self.plant.num_velocities()) return PiecewisePolynomial.FirstOrderHold([0.,1.], traj) elif type(traj) is PiecewisePolynomial: breaks = traj.get_segment_times() values = traj.vector_values(breaks) values = zero_pad_rows(values, self.plant.num_positions() + self.plant.num_velocities()) return PiecewisePolynomial.FirstOrderHold(breaks, values) else: raise ValueError("Trajectory must be a piecewise polynomial, an ndarray, or None")
def test_simple_trajectory(self): plant = MultibodyPlant(0.001) load_atlas(plant, add_ground=False) T = 2.0 l_foot_pos_traj = PiecewisePolynomial.FirstOrderHold( np.linspace(0, T, 5), np.array([[0.00, 0.09, 0.00], [0.25, 0.09, 0.10], [0.50, 0.09, 0.00], [0.50, 0.09, 0.00], [0.50, 0.09, 0.00]]).T) r_foot_pos_traj = PiecewisePolynomial.FirstOrderHold( np.linspace(0, T, 5), np.array([[0.00, -0.09, 0.00], [0.00, -0.09, 0.00], [0.00, -0.09, 0.00], [0.25, -0.09, 0.10], [0.50, -0.09, 0.00]]).T) pelvis_pos_traj = PiecewisePolynomial.FirstOrderHold( [0.0, T], np.array([[0.00, 0.00, Atlas.PELVIS_HEIGHT - 0.05], [0.50, 0.00, Atlas.PELVIS_HEIGHT - 0.05]]).T) null_orientation_traj = PiecewiseQuaternionSlerp([0.0, T], [ Quaternion([1.0, 0.0, 0.0, 0.0]), Quaternion([1.0, 0.0, 0.0, 0.0]) ]) generator = PoseGenerator(plant, [ Trajectory('l_foot', Atlas.FOOT_OFFSET, l_foot_pos_traj, 1e-3, null_orientation_traj, 0.05), Trajectory('r_foot', Atlas.FOOT_OFFSET, r_foot_pos_traj, 1e-3, null_orientation_traj, 0.05), Trajectory('pelvis', np.zeros(3), pelvis_pos_traj, 1e-2, null_orientation_traj, 0.2) ]) for t in np.linspace(0, T, 50): q = generator.get_ik(t) if q is not None: visualize(q) else: self.fail("Failed to find IK solution!") time.sleep(0.2)
def add_trajectory(self, time_steps, time_interval): ''' Generate a starting trajectory guess by drawing a line from start to target. ''' # initial and final time and state time_limits = [0., time_steps * time_interval] position_limits = np.column_stack((self.start, self.end)) # linear interpolation in state state = PiecewisePolynomial.FirstOrderHold(time_limits, position_limits) # sample state on the time grid return np.vstack([state.value(t * time_interval).T for t in range(time_steps+1)])
def get_figure_eight_traj(tree, tspan, q_init=None, q_final=None): assert tspan[-1] - tspan[0] >= 10, \ "tspan should be at least 10 seconds long, 30 seconds is recommended" Nq = tree.get_num_positions() eps = 1e-4 # # initial pose constraint # constraints = [] # if q_init is None: # q_init = getKukaHome() # initial_pose_constraint = PostureConstraint( # tree, tspan[0] + np.array([-eps, eps])) # initial_pose_constraint.setJointLimits( # range(6), q_init - eps, q_init + eps) # constraints.append(initial_pose_constraint) # # final post constraint # if q_final is None: # q_final = getKukaHome() # final_pose_constraint = PostureConstraint( # tree, tspan[-1] + np.array([-eps, eps])) # final_pose_constraint.setJointLimits( # range(6), q_final - eps, q_final + eps) # constraints.append(final_pose_constraint) # constraints for figure eight t_traj = np.linspace(tspan[0], tspan[-1], 200) constraints.extend(get_figure_eight_traj_constraints(tree, t_traj)) # compute inverse kinematics # t = np.concatenate([[tspan[0]], t_traj, [tspan[-1]]]) ik_options = IKoptions(tree) ik_options.setFixInitialState(False) ik_options.setMajorFeasibilityTolerance(1e-5) q_seed = np.array([getKukaHome() for _ in t]).T res = InverseKinTraj(tree, t, q_seed, q_seed, constraints, ik_options) q_sol = np.array(res.q_sol).T print getKukaHome() for i in range(q_sol.shape[1]): print q_sol[:, i] return PiecewisePolynomial.Cubic(t, q_sol, np.zeros(Nq), np.zeros(Nq))
def interpolate_rocket_state(p_initial, p_final, time_steps): np.random.seed(1) # initial and final time and state time_limits = [0., time_steps * time_interval] position_limits = np.column_stack((p_initial, p_final)) state_limits = np.vstack((position_limits, np.zeros((2, 2)))) # linear interpolation in state state = PiecewisePolynomial.FirstOrderHold(time_limits, state_limits) # sample state on the time grid and add small random noise state_guess = np.vstack([state.value(t * time_interval).T for t in range(time_steps + 1)]) state_guess += np.random.rand(*state_guess.shape) * 1e-3 return state_guess
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument("-t1", default=0.05, help="Extend leg") parser.add_argument("-t2", default=0.5, help="Dwell at top") parser.add_argument("-t3", default=0.5, help="Contract leg") parser.add_argument("-t4", default=0.1, help="Wait at bottom") setup_argparse_for_setup_dot_diagram(parser) MeshcatVisualizer.add_argparse_argument(parser) args = parser.parse_args() t1 = float(args.t1) t2 = float(args.t2) t3 = float(args.t3) t4 = float(args.t4) q_crouch = np.array([ 1600, 2100, 2000, 1600, 2100, 2000, 1400, 2100, 2000, 1400, 2100, 2000 ]) q_extend = np.array([ 1600, 1600, 2400, 1600, 1600, 2400, 1400, 1600, 2400, 1400, 1600, 2400 ]) breaks = np.cumsum([0., t1, t2, t3, t4]) samples = np.stack([q_crouch, q_extend, q_extend, q_crouch, q_crouch]).T trajectory = PiecewisePolynomial.FirstOrderHold(breaks, samples) builder = DiagramBuilder() plant, scene_graph, servo_controller = setup_dot_diagram(builder, args) trajectory_source = builder.AddSystem(TrajectoryLooper(trajectory)) builder.Connect(trajectory_source.get_output_port(0), servo_controller.get_input_port(0)) if args.meshcat: meshcat = ConnectMeshcatVisualizer( builder, output_port=scene_graph.get_query_output_port(), zmq_url=args.meshcat, open_browser=args.open_browser) diagram = builder.Build() simulator = Simulator(diagram) simulator.set_target_realtime_rate(1.0) simulator.AdvanceTo(1E6)
def LQR(self, xtraj, utraj, Q, R, Qf): tspan = utraj.get_segment_times() context = self.CreateDefaultContext() nX = context.num_continuous_states() nU = self.GetInputPort('u').size() sym_system = self.ToSymbolic() sym_context = sym_system.CreateDefaultContext() prog = MathematicalProgram() x = prog.NewIndeterminates(nX, 'x') ucon = prog.NewIndeterminates(nU, 'u') #nY = self.GetOutputPort('x').size() #N = np.zeros([nX, nU]) times = xtraj.get_segment_times() K = [] import pdb pdb.set_trace() for t in times: # option 1 x0 = xtraj.value(t).transpose()[0] u0 = utraj.value(t).transpose()[0] sym_context.SetContinuousState(x0 + x) sym_context.FixInputPort(0, u0 + ucon) f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector() A = Evaluate(Jacobian(f, x), dict(zip(x, x0))) B = Evaluate(Jacobian(f, ucon), dict(zip(ucon, u0))) K.append(LinearQuadraticRegulator(A, B, Q, R)) # option 2 #context.SetContinuousState(xtraj.value(t) ) #context.FixInputPort(0, utraj.value(t) ) #linearized_plant = Linearize(self, context) #K.append(LinearQuadraticRegulator(linearized_plant.A(), # linearized_plant.B(), # Q, R)) #self, context, Q, R) Kpp = PiecewisePolynomial.FirstOrderHold(times, K) return Kpp
def cb(huxT): print(" {}".format(self.vis_cb_counter), end='') if (self.vis_cb_counter) % every_nth != 0: return #print() # Unpack the serialized variables num_h = self.num_trajectories num_u = self.num_trajectories*self.num_samples*self.num_inputs num_x = self.num_trajectories*self.num_samples*self.num_states h = huxT[ : num_h ].reshape((self.num_trajectories, 1)) u = huxT[num_h : num_h+num_u ].reshape((self.num_trajectories, self.num_samples, self.num_inputs)) x = huxT[num_h+num_u : num_h+num_u+num_x].reshape((self.num_trajectories, self.num_samples, self.num_states)) # Swap last two axes here to get it into the format we're used to above u = np.swapaxes(u, 1, 2) x = np.swapaxes(x, 1, 2) T = huxT[num_h+num_u+num_x : ] # Visualize the trajectories for ti in range(self.num_trajectories): h_sol = h[ti][0] if h_sol <= 0: print("bad h_sol") continue breaks = [h_sol*i for i in range(self.num_samples)] knots = x[ti] x_trajectory = PiecewisePolynomial.Cubic(breaks, knots, False) t_samples = np.linspace(breaks[0], breaks[-1], self.num_samples*3) x_samples = np.hstack([x_trajectory.value(t) for t in t_samples]) # 1) Visualize the trajectories plot_trajectory(x_samples, "state_scatter", self.expmt, create_figure=False) if len(T) != 0: for ic in vis_ic_list: # 2) Then visualize what the policy would say to do from (possibly the same) initial conditions. h_sol = (0.01+0.2)/2 # TODO: improve this hard coding? _, pi_x_samples, _ = self.__rollout_policy_given_params(h_sol, T, ic) plot_trajectory(pi_x_samples, "state_scatter", self.expmt, create_figure=False, symbol=':') plt.show()
def interpolate_dubins_state(x0, xf, time_steps, time_interval, r): ''' Creates an initial guess for the state by drawing a straight line between the initial and final positions ''' np.random.seed(0) # initial and final time and state time_limits = [0., time_steps * time_interval] position_limits = np.column_stack((x0, xf + np.array([0, r, -np.pi]))) # set final position to be the top of the circle state_limits = position_limits # linear interpolation in state state = PiecewisePolynomial.FirstOrderHold(time_limits, state_limits) # sample state on the time grid and add small random noise state_guess = np.vstack([state.value(t * time_interval).T for t in range(time_steps + 1)]) state_guess += np.random.rand(*state_guess.shape) * 9e-1 # print(np.shape(state_guess)) return state_guess
def drake_trajectory_generation(file_name): global x_cmd_drake global u_cmd_drake print(file_name) Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() global dircol dircol= DirectCollocation( plant, context, num_time_samples=11, minimum_timestep=0.1, maximum_timestep=0.4, input_port_index=plant.get_actuation_input_port().get_index()) dircol.AddEqualTimeIntervalsConstraints() initial_state = (0., 0., 0., 0.) dircol.AddBoundingBoxConstraint(initial_state, initial_state, dircol.initial_state()) final_state = (0., math.pi, 0., 0.) dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state()) R = 10 # Cost on input "effort".weight u = dircol.input() dircol.AddRunningCost(R * u[0]**2) # Add a final cost equal to the total duration. dircol.AddFinalCost(dircol.time()) initial_x_trajectory = PiecewisePolynomial.FirstOrderHold( [0., 4.], np.column_stack((initial_state, final_state))) # yapf: disable dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) dircol.AddConstraintToAllKnotPoints(dircol.input()[1] <= 0) dircol.AddConstraintToAllKnotPoints(dircol.input()[1] >= 0) global result global u_values result = Solve(dircol) assert result.is_success() #plotphase_portrait() fig1, ax1 = plt.subplots() u_trajectory = dircol.ReconstructInputTrajectory(result) u_knots = np.hstack([ u_trajectory.value(t) for t in np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 400) ])#here the u_knots now is 2x400 #u_trajectory = dircol.ReconstructInputTrajectory(result) times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 400) #u_lookup = np.vectorize(u_trajectory.value) #now we have ndarray of u_values with 400 points for 4 seconds w/ 100hz pub frequency #u_values = u_lookup(times) #ax1.plot(times, u_values) ax1.plot(times, u_knots[0]) ax1.plot(times, u_knots[1]) ax1.set_xlabel("time (seconds)") ax1.set_ylabel("force (Newtons)") ax1.set_title(' Direct collocation for Cartpole ') print('here2') plt.show() print('here3') #x_knots = np.hstack([ # x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(), # x_trajectory.end_time(), 100) #]) x_trajectory = dircol.ReconstructStateTrajectory(result) x_knots = np.hstack([ x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(), x_trajectory.end_time(), 400) ]) print(x_trajectory.start_time()) print(x_trajectory.end_time()) fig, ax = plt.subplots(4,1,figsize=(8,8)) plt.subplots_adjust(wspace =0, hspace =0.4) #plt.tight_layout(3)#adjust total space ax[0].set_title('state of direct collocation for Cartpole') ax[0].plot(x_knots[0, :], x_knots[2, :], linewidth=2, color='b', linestyle='-') ax[0].set_xlabel("state_dot(theta1_dot and t|heta2_dot)") ax[0].set_ylabel("state(theta1 and theta2)"); ax[0].plot(x_knots[1, :], x_knots[3, :],color='r',linewidth=2,linestyle='--') ax[0].legend(('theta1&theta1dot','theta2&theta2dot')); ax[1].set_title('input u(t) of direct collocation for Cartpole') # ax[1].plot(times,u_values, 'g') ax[1].plot(times, u_knots[0]) ax[1].plot(times, u_knots[1]) ax[1].legend(('input u(t)')) ax[1].set_xlabel("time") ax[1].set_ylabel("u(t)") ax[1].legend(('x joint ','thetajoint')) ax[1].set_title('input x(t) of direct collocation for Cartpole') ax[2].plot(times, x_knots[0, :]) ax[2].set_xlabel("time") ax[2].set_ylabel("x(t)") ax[2].set_title('input theta(t) of direct collocation for Cartpole') ax[3].set_title('input theta(t) of direct collocation for Cartpole') ax[3].plot(times, x_knots[1, :]) ax[3].set_xlabel("time") ax[3].set_ylabel("theta(t)") print('here4') plt.show() print('here5') x_cmd_drake=x_knots #return x_knots[0, :]#u_values # u_cmd_drake=u_values u_cmd_drake=u_knots
# More elegant version is blocked on drake #8315: # dircol.AddLinearConstraint(dircol.initial_state() == initial_state) final_state = (math.pi, 0., 0., 0.) dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state()) # dircol.AddLinearConstraint(dircol.final_state() == final_state) R = 10 # Cost on input "effort". dircol.AddRunningCost(R * u[0]**2) # Add a final cost equal to the total duration. dircol.AddFinalCost(dircol.time()) initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., 4.], np.column_stack((initial_state, final_state))) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) result = Solve(dircol) print(result.get_solver_id().name()) print(result.get_solution_result()) assert (result.is_success()) x_trajectory = dircol.ReconstructStateTrajectory(result) tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"), FloatingBaseType.kFixed) vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.]) ani = vis.animate(x_trajectory, repeat=True)
0:x.shape[0]] rbt_new, x = world_builder.do_cut(rbt, x, cut_body_index=e.cut_body_index, cut_pt=e.cut_pt, cut_normal=e.cut_normal) except StopIteration: print "Terminated early" except RuntimeError as e: print "Runtime Error: ", e print "Probably NAN in simulation. Terminating early." sim_slices.append(( rbt, PiecewisePolynomial.FirstOrderHold( # Discard first knot, as it's repeated state_log.sample_times()[1:], state_log.data()[:, 1:]))) if rbt_new: # Cloning one RBT into another only sort of works; # instead force rbt to become a reference to rbt_new, # and let rbt get garbage collected. rbt = None rbt = rbt_new else: break print("Final state: ", state.CopyToVector()) end_time = simulator.get_mutable_context().get_time() if args.animate_forever: try:
def make_dircol_pendulum(ic=(-1., 0.), num_samples=32, min_timestep=0.002, max_timestep=0.25, warm_start="linear", seed=1776, should_vis=False, target_traj=None, **kwargs): # if 'warm_start' in kwargs: # print(kwargs['warm_start']) # else: # print("warm_start", warm_start) global dircol global plant global context plant = PendulumPlant() context = plant.CreateDefaultContext() dircol = DirectCollocation(plant, context, num_time_samples=num_samples, minimum_timestep=min_timestep, maximum_timestep=max_timestep) dircol.AddEqualTimeIntervalsConstraints() # torque_limit = input_limit # N*m. torque_limit = 5. u = dircol.input() dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0]) dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit) initial_state = ic dircol.AddBoundingBoxConstraint(initial_state, initial_state, dircol.initial_state()) final_state = (math.pi, 0.) dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state()) # R = 100 # Cost on input "effort". u = dircol.input() x = dircol.state() # print(x) dircol.AddRunningCost(2 * ((x[0] - math.pi) * (x[0] - math.pi) + x[1] * x[1]) + 25 * u.dot(u)) # Add a final cost equal to the total duration. # dircol.AddFinalCost(dircol.time()) if warm_start == "linear": initial_u_trajectory = PiecewisePolynomial() initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., 4.], np.column_stack((initial_state, final_state))) dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory) elif warm_start == "random": assert isinstance(seed, int) np.random.seed(seed) breaks = np.linspace(0, 4, num_samples).reshape( (-1, 1)) # using num_time_samples u_knots = np.random.rand( 1, num_samples) - 0.5 # num_inputs vs num_samples? x_knots = np.random.rand( 2, num_samples) - 0.5 # num_states vs num_samples? initial_u_trajectory = PiecewisePolynomial.Cubic( breaks, u_knots, False) initial_x_trajectory = PiecewisePolynomial.Cubic( breaks, x_knots, False) dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory) elif warm_start == "target": assert target_traj != [], "Need a valid target for warm starting" (breaks, x_knots, u_knots) = target_traj #(breaks, u_knots, x_knots) = target_traj initial_u_trajectory = PiecewisePolynomial.Cubic( breaks.T, u_knots.T, False) initial_x_trajectory = PiecewisePolynomial.Cubic( breaks.T, x_knots.T, False) dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory) def cb(decision_vars): global vis_cb_counter vis_cb_counter += 1 if vis_cb_counter % 10 != 0: return # Get the total cost all_costs = dircol.EvalBindings(dircol.GetAllCosts(), decision_vars) # Get the total cost of the constraints. # Additionally, the number and extent of any constraint violations. violated_constraint_count = 0 violated_constraint_cost = 0 constraint_cost = 0 for constraint in dircol.GetAllConstraints(): val = dircol.EvalBinding(constraint, decision_vars) # Consider switching to DoCheckSatisfied if you can find the binding... nudge = 1e-1 # This much constraint violation is not considered bad... lb = constraint.evaluator().lower_bound() ub = constraint.evaluator().upper_bound() good_lb = np.all(np.less_equal(lb, val + nudge)) good_ub = np.all(np.greater_equal(ub, val - nudge)) if not good_lb or not good_ub: # print("{} <= {} <= {}".format(lb, val, ub)) violated_constraint_count += 1 # violated_constraint_cost += np.sum(np.abs(val)) if not good_lb: violated_constraint_cost += np.sum(np.abs(lb - val)) if not good_ub: violated_constraint_cost += np.sum(np.abs(val - ub)) constraint_cost += np.sum(np.abs(val)) print("total cost: {: .2f} | \tconstraint {: .2f} \tbad {}, {: .2f}". format(sum(all_costs), constraint_cost, violated_constraint_count, violated_constraint_cost)) #dircol.AddVisualizationCallback(cb, dircol.decision_variables()) def MyVisualization(sample_times, values): global vis_cb_counter vis_cb_counter += 1 #print("counter: ", vis_cb_counter) if vis_cb_counter % 10 != 0: return x, x_dot = values[0], values[1] plt.plot(x, x_dot, '-o', label=vis_cb_counter) plt.show() if should_vis: plt.figure() plt.title('Tip trajectories') plt.xlabel('x') plt.ylabel('x_dot') dircol.AddStateTrajectoryCallback(MyVisualization) from pydrake.all import (SolverType) #dircol.SetSolverOption(SolverType.kSnopt, 'Major feasibility tolerance', 1.0e-6) # default="1.0e-6" #dircol.SetSolverOption(SolverType.kSnopt, 'Major optimality tolerance', 1.0e-6) # default="1.0e-6" #dircol.SetSolverOption(SolverType.kSnopt, 'Minor feasibility tolerance', 1.0e-6) # default="1.0e-6" #dircol.SetSolverOption(SolverType.kSnopt, 'Minor optimality tolerance', 1.0e-6) # default="1.0e-6" # dircol.SetSolverOption(SolverType.kSnopt, 'Major feasibility tolerance', 1.0e-6) # default="1.0e-6" # dircol.SetSolverOption(SolverType.kSnopt, 'Major optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1 # dircol.SetSolverOption(SolverType.kSnopt, 'Minor feasibility tolerance', 1.0e-6) # default="1.0e-6" # dircol.SetSolverOption(SolverType.kSnopt, 'Minor optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1 dircol.SetSolverOption( SolverType.kSnopt, 'Time limit (secs)', 60.0) # default="9999999.0" # Very aggressive cutoff... dircol.SetSolverOption( SolverType.kSnopt, 'Major step limit', 0.1) # default="2.0e+0" # HUGE!!! default takes WAY too huge steps # dircol.SetSolverOption(SolverType.kSnopt, 'Reduced Hessian dimension', 10000) # Default="min{2000, n1 + 1}" # dircol.SetSolverOption(SolverType.kSnopt, 'Hessian updates', 30) # Default="10" dircol.SetSolverOption(SolverType.kSnopt, 'Major iterations limit', 9300000) # Default="9300" dircol.SetSolverOption(SolverType.kSnopt, 'Minor iterations limit', 50000) # Default="500" dircol.SetSolverOption(SolverType.kSnopt, 'Iterations limit', 50 * 10000) # Default="10000" # Factoriztion? # dircol.SetSolverOption(SolverType.kSnopt, 'QPSolver Cholesky', True) # Default="*Cholesky/CG/QN" #dircol.SetSolverOption(SolverType.kSnopt, 'Major iterations limit', 1) # Default="9300" #dircol.SetSolverOption(SolverType.kSnopt, 'Minor iterations limit', 1) # Default="500" return dircol
axs1[2].set_xlabel('Time (s)') # one collision point fig3, axs3 = plt.subplots(3, 1) axs3[0].plot(t, l[0, :], linewidth=1.5) axs3[0].set_ylabel('Normal') axs3[0].set_title('Ground reaction forces') axs3[1].plot(t, l[1, :] - l[3, :], linewidth=1.5) axs3[1].set_ylabel('Friction-x') axs3[2].plot(t, l[2, :] - l[4, :], linewidth=1.5) axs3[2].set_ylabel('Friction-y') # axs3[2].set_ylim(-0.5, 3) axs3[2].set_xlabel('Time (s)') # Show the plots plt.show() print('Done!') x = PiecewisePolynomial.FirstOrderHold(t, x) vis = Visualizer(_file) body_inds = vis.plant.GetBodyIndices(vis.model_index) base_frame = vis.plant.get_body(body_inds[0]).body_frame() vis.plant.WeldFrames(vis.plant.world_frame(), base_frame, RigidTransform()) vis.visualize_trajectory(x) # Save the results # file = "data/slidingblock/block_trajopt.pkl" # data = trajopt.result_to_dict(result) # utils.save(file, data)
# == initial_state.get_value()) final_state = PendulumState() final_state.set_theta(math.pi) final_state.set_thetadot(0.0) dircol.AddBoundingBoxConstraint(final_state.get_value(), final_state.get_value(), dircol.final_state()) # dircol.AddLinearConstraint(dircol.final_state() == final_state.get_value()) R = 10 # Cost on input "effort". dircol.AddRunningCost(R*u[0]**2) initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., 4.], [initial_state.get_value(), final_state.get_value()]) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) result = Solve(dircol) assert result.is_success() x_trajectory = dircol.ReconstructStateTrajectory(result) fig, ax = plt.subplots(1, 2) vis = PendulumVisualizer(ax[0]) ani = vis.animate(x_trajectory, repeat=True) x_knots = np.hstack([x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
def plan_grasping_trajectory(rbt, q0, target_reach_pose, target_grasp_pose, n_knots, reach_time, grasp_time): ''' Solves IK at a series of sample times (connected with a cubic spline) to generate a trajectory to bring the Kuka from an initial pose q0 to a final end effector pose in the specified time, using the specified number of knot points. Uses an intermediate pose reach_pose as an intermediate target to hit at the knot point closest to reach_time. See http://drake.mit.edu/doxygen_cxx/rigid__body__ik_8h.html for the "inverseKinTraj" entry. At the moment, the Python binding for this function uses "inverseKinTrajSimple" -- i.e., it doesn't return derivatives. ''' nq = rbt.get_num_positions() q_des_full = np.zeros(nq) # Create knot points ts = np.linspace(0., grasp_time, n_knots) # Figure out the knot just before reach time reach_start_index = np.argmax(ts >= reach_time) - 1 controlled_joint_names = [ "iiwa_joint_1", "iiwa_joint_2", "iiwa_joint_3", "iiwa_joint_4", "iiwa_joint_5", "iiwa_joint_6", "iiwa_joint_7" ] free_config_inds, constrained_config_inds = \ kuka_utils.extract_position_indices(rbt, controlled_joint_names) # Assemble IK constraints constraints = [] # Constrain the non-searched-over joints for all time all_tspan = np.array([0., grasp_time]) posture_constraint = ik.PostureConstraint(rbt, all_tspan) posture_constraint.setJointLimits(constrained_config_inds, q0[constrained_config_inds] - 0.01, q0[constrained_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain all joints to be the initial posture at the start time start_tspan = np.array([0., 0.]) posture_constraint = ik.PostureConstraint(rbt, start_tspan) posture_constraint.setJointLimits(free_config_inds, q0[free_config_inds] - 0.01, q0[free_config_inds] + 0.01) constraints.append(posture_constraint) # Constrain the ee frame to lie on the target point # facing in the target orientation in between the # reach and final times ee_frame = rbt.findFrame("iiwa_frame_ee").get_frame_index() for i in range(reach_start_index, n_knots): this_tspan = np.array([ts[i], ts[i]]) interp = float(i - reach_start_index) / (n_knots - reach_start_index) target_pose = (1. - interp) * target_reach_pose + interp * target_grasp_pose constraints.append( ik.WorldPositionConstraint(rbt, ee_frame, np.zeros((3, 1)), target_pose[0:3] - 0.01, target_pose[0:3] + 0.01, tspan=this_tspan)) constraints.append( ik.WorldEulerConstraint(rbt, ee_frame, target_pose[3:6] - 0.05, target_pose[3:6] + 0.05, tspan=this_tspan)) # Seed and nom are both the initial repeated for the # # of knot points q_seed = np.tile(q0, [1, n_knots]) q_nom = np.tile(q0, [1, n_knots]) options = ik.IKoptions(rbt) # Set bounds on initial and final velocities zero_velocity = np.zeros(rbt.get_num_velocities()) options.setqd0(zero_velocity, zero_velocity) options.setqdf(zero_velocity, zero_velocity) results = ik.InverseKinTraj(rbt, ts, q_seed, q_nom, constraints, options) qtraj = PiecewisePolynomial.Pchip(ts, np.vstack(results.q_sol).T, True) print "IK returned a solution with info %d" % results.info[0] print "(Info 1 is good, other values are dangerous)" return qtraj, results.info[0]
All the other optimization variables are initialized at zero. (Note that, if the initial guess for a variable is not specified, the default value is zero.) """ # vector of the initial guess initial_guess = np.empty(prog.num_vars()) # initial guess for the time step h_guess = h_max prog.SetDecisionVariableValueInVector(h, [h_guess] * T, initial_guess) # linear interpolation of the configuration q0_guess = np.array([0, 0, .15, -.3]) q_guess_poly = PiecewisePolynomial.FirstOrderHold( [0, T * h_guess], np.column_stack((q0_guess, - q0_guess)) ) qd_guess_poly = q_guess_poly.derivative() qdd_guess_poly = q_guess_poly.derivative() # set initial guess for configuration, velocity, and acceleration q_guess = np.hstack([q_guess_poly.value(t * h_guess) for t in range(T + 1)]).T qd_guess = np.hstack([qd_guess_poly.value(t * h_guess) for t in range(T + 1)]).T qdd_guess = np.hstack([qdd_guess_poly.value(t * h_guess) for t in range(T)]).T prog.SetDecisionVariableValueInVector(q, q_guess, initial_guess) prog.SetDecisionVariableValueInVector(qd, qd_guess, initial_guess) prog.SetDecisionVariableValueInVector(qdd, qdd_guess, initial_guess) # initial guess for the normal component of the stance-leg force bodies = ['body', 'stance_leg', 'swing_leg'] mass = sum(compass_gait.GetBodyByName(body).default_mass() for body in bodies)
dircol.AddEqualTimeIntervalsConstraints() # Initial state on the surface of section (and velocity > .1). dircol.AddBoundingBoxConstraint([0., 0.1], [0., 10.], dircol.initial_state()) # Periodicity constraint. # TODO(russt): Replace this with the vectorized version pending drake #8315. dircol.AddLinearConstraint( dircol.final_state()[0] == dircol.initial_state()[0]) dircol.AddLinearConstraint( dircol.final_state()[1] == dircol.initial_state()[1]) samples = np.linspace(0, 2*math.pi, 10) x_guess = np.vstack(([2*math.sin(t) for t in samples], [2*math.cos(t) for t in samples])) initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(samples, x_guess) dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory) fig = plt.figure() h, = plt.plot([], [], '.-') plt.xlim((-2.5, 2.5)) plt.ylim((-3., 3.)) def draw_trajectory(t, x): h.set_xdata(x[0, :]) h.set_ydata(x[1, :]) fig.canvas.draw() if plt.get_backend() == u'MacOSX': plt.pause(1e-10)
def plan_throw( T_world_robotInitial, T_world_gripperObject, p_world_target, # np.array of shape (3,) gripper_to_object_dist, throw_height=0.5, # meters prethrow_height=0.2, prethrow_radius=0.4, throw_angle=np.pi / 4.0, meshcat=None, throw_speed_adjustment_factor=1.0, ): """ only works with the "back portion" of the clutter station until we figure out how to move the bins around motion moves along an arc from a "pre throw" to a "throw" position """ theta = 1.0 * np.arctan2(p_world_target[1], p_world_target[0]) print(f"theta={theta}") T_world_prethrow = RigidTransform( p=np.array([ prethrow_radius * np.cos(theta), prethrow_radius * np.sin(theta), prethrow_height ]), R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply( RotationMatrix.MakeYRotation((theta - np.pi / 2)))) throw_radius = throw_height - prethrow_height T_world_throw = RigidTransform( p=T_world_prethrow.translation() + np.array([ throw_radius * np.cos(theta), throw_radius * np.sin(theta), throw_height - prethrow_height ]), R=RotationMatrix.MakeXRotation(-np.pi / 2).multiply( RotationMatrix.MakeYRotation((theta - np.pi / 2)).multiply( RotationMatrix.MakeXRotation(-np.pi / 2)))) if meshcat: visualize_transform(meshcat, "T_world_prethrow", T_world_prethrow) visualize_transform(meshcat, "T_world_throw", T_world_throw) p_world_object_at_launch = interpolatePosesArcMotion( T_world_prethrow, T_world_throw, t=throw_angle / (np.pi / 2.)).translation() + np.array([0, 0, -gripper_to_object_dist]) pdot_world_launch = interpolatePosesArcMotion_pdot(T_world_prethrow, T_world_throw, t=throw_angle / (np.pi / 2.)) launch_speed_base = np.linalg.norm(pdot_world_launch) launch_speed_required = get_launch_speed_required( theta=throw_angle, x=np.linalg.norm(p_world_target[:2]) - np.linalg.norm(p_world_object_at_launch[:2]), y=p_world_target[2] - p_world_object_at_launch[2]) total_throw_time = launch_speed_base / launch_speed_required / throw_speed_adjustment_factor print(f"p_world_object_at_launch={p_world_object_at_launch}") print(f"target={p_world_target}") print( f"dx={np.linalg.norm(p_world_target[:2]) - np.linalg.norm(p_world_object_at_launch[:2])}" ) print(f"dy={p_world_target[2] - p_world_object_at_launch[2]}") print(f"pdot_world_launch={pdot_world_launch}") print(f"total_throw_time={total_throw_time}") T_world_hackyWayPoint = RigidTransform( p=[-.6, -0.0, 0.6], R=RotationMatrix.MakeXRotation( -np.pi / 2.0 ), #R_WORLD_PRETHROW, #RotationMatrix.MakeXRotation(-np.pi/2.0), ) # event timings (not all are relevant to pose and gripper) # initial pose => prethrow => throw => yays t_goToObj = 1.0 t_holdObj = 0.5 t_goToPreobj = 1.0 t_goToWaypoint = 1.0 t_goToPrethrow = 1.0 t_goToRelease = total_throw_time * throw_angle / (np.pi / 2.) t_goToThrowEnd = total_throw_time * (1 - throw_angle / (np.pi / 2.)) t_throwEndHold = 3.0 ts = np.array([ t_goToObj, t_holdObj, t_goToPreobj, t_goToWaypoint, t_goToPrethrow, t_goToRelease, t_goToThrowEnd, t_throwEndHold ]) cum_pose_ts = np.cumsum(ts) print(cum_pose_ts) # Create pose trajectory t_lst = np.linspace(0, cum_pose_ts[-1], 1000) pose_lst = [] for t in t_lst: if t < cum_pose_ts[1]: pose = interpolatePosesLinear(T_world_robotInitial, T_world_gripperObject, min(t / ts[0], 1.0)) elif t < cum_pose_ts[2]: pose = interpolatePosesLinear(T_world_gripperObject, T_world_robotInitial, (t - cum_pose_ts[1]) / ts[2]) elif t < cum_pose_ts[3]: pose = interpolatePosesLinear(T_world_robotInitial, T_world_hackyWayPoint, (t - cum_pose_ts[2]) / ts[3]) elif t <= cum_pose_ts[4]: pose = interpolatePosesLinear(T_world_hackyWayPoint, T_world_prethrow, (t - cum_pose_ts[3]) / ts[4]) else: pose = interpolatePosesArcMotion( T_world_prethrow, T_world_throw, min((t - cum_pose_ts[4]) / (ts[5] + ts[6]), 1.0)) pose_lst.append(pose) # Create gripper trajectory. gripper_times_lst = np.array([ 0., t_goToObj, t_holdObj, t_goToPreobj, t_goToWaypoint, t_goToPrethrow, t_goToRelease, t_goToThrowEnd, t_throwEndHold ]) gripper_cumulative_times_lst = np.cumsum(gripper_times_lst) GRIPPER_OPEN = 0.5 GRIPPER_CLOSED = 0.0 gripper_knots = np.array([ GRIPPER_OPEN, GRIPPER_OPEN, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_OPEN, GRIPPER_CLOSED ]).reshape(1, gripper_times_lst.shape[0]) g_traj = PiecewisePolynomial.FirstOrderHold(gripper_cumulative_times_lst, gripper_knots) return t_lst, pose_lst, g_traj
def TVLQR(self, xtraj, utraj, Q, R, Qf): #context = self.CreateDefaultContext() #sym_system = self.ToSymbolic() #sym_context = sym_system.CreateDefaultContext() #prog = MathematicalProgram() #x = prog.NewIndeterminates(self.nX,'x') #ucon = prog.NewIndeterminates(self.nU,'u') times = xtraj.get_segment_times() # final state goal t = times[-1] x0 = xtraj.value(t).transpose()[0] u0 = utraj.value(t).transpose()[0] #sym_context.SetContinuousState(x) #x0+ #sym_context.FixInputPort(0, ucon ) #u0+ # xdot=f(x,u)==>xhdot=f(xh+x0,uh+u0)-x0dot ~= df/dx*xh + df/du*uh #f = sym_system.EvalTimeDerivatives(sym_context).CopyToVector() #mapping = dict(zip(x, x0)) #mapping.update(dict(zip(ucon, u0))) # get the final condition, according to Tedrake10' Qf should be last S(LTI) #A_sim = Jacobian(f, x) #B_sim = Jacobian(f, ucon) #A = Evaluate(A_sim, mapping) #B = Evaluate(B_sim, mapping) #k,s = LinearQuadraticRegulator(A, B, Q, R) # get the Qf = S_lti #Qf = s # now, run backwards in time to solve for S for associated Riccati equation Rinv = np.linalg.inv(R) #S(3x3),s1(3,1),s2(1x1) #S0 = np.array(Qf.ravel().tolist()+[0.0,0.0,0.0,0.0]) S0 = Qf.ravel() #import pdb; pdb.set_trace() sol = solve_ivp(lambda t,S: self.Cdre(t,S,Q,Rinv,xtraj,utraj), \ [times[-1], times[0]], S0, t_eval=times[::-1]) K = [] S = [] S_debug = [] Ssol = np.fliplr(sol.y) # flip is because we switch back to normal time progress for i in range(sol.y.shape[1]): x0 = xtraj.value(times[i]).transpose()[0] u0 = utraj.value(times[i]).transpose()[0] A, B = self.PlantDerivatives(x0, u0) s = Ssol[:,i].reshape(Qf.shape) k = Rinv.dot(B.transpose()).dot(s) if(len(K) == 0): K = np.ravel(k).reshape(self.nX*self.nU,1) S_debug = np.ravel(s).reshape(self.nX*self.nX,1) else: K = np.hstack( (K, np.ravel(k).reshape(self.nX*self.nU,1)) ) S_debug = np.hstack( (S_debug, np.ravel(s).reshape(self.nX*self.nX,1)) ) Kpp = PiecewisePolynomial.FirstOrderHold(times, K) Spp_debug = PiecewisePolynomial.FirstOrderHold(times, S_debug) # now that it's an autonomous system, get the Lyapunov function ''' prog = MathematicalProgram() x = prog.NewIndeterminates(self.nX, 'x') ucon = prog.NewIndeterminates(self.nU, 'u') sol = solve_ivp(lambda t,S: self.Cdre_CL(t,S,Q,Rinv,xtraj,utraj,x,ucon,Kpp), \ [times[-1], times[0]], S0, t_eval=times[::-1]) Ssol = np.fliplr(sol.y) # flip is because we switch back to normal time progress for i in range(sol.y.shape[1]): s = Ssol[:,i].reshape(Qf.shape) if(len(S) == 0): S = np.ravel(s).reshape(self.nX*self.nX,1) else: S = np.hstack( (S, np.ravel(s).reshape(self.nX*self.nX,1)) ) Spp = PiecewisePolynomial.FirstOrderHold(times, S) ''' #import pdb; pdb.set_trace() return Kpp,Spp_debug #Spp
def make_dircol_cartpole(ic=(-1., 0., 0., 0.), num_samples=21, min_timestep=0.0001, max_timestep=1., warm_start="linear", seed=1776, should_vis=False, torque_limit=250., target_traj=None, **kwargs): global dircol global plant global context tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf", FloatingBaseType.kFixed) plant = RigidBodyPlant(tree) context = plant.CreateDefaultContext() dircol = DirectCollocation( plant, context, num_time_samples=num_samples, # minimum_timestep=0.01, maximum_timestep=0.01) minimum_timestep=min_timestep, maximum_timestep=max_timestep) # dircol.AddEqualTimeIntervalsConstraints() # torque_limit = input_limit # N*m. # torque_limit = 64. u = dircol.input() dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0]) dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit) initial_state = ic dircol.AddBoundingBoxConstraint(initial_state, initial_state, dircol.initial_state()) final_state = np.array([0., math.pi, 0., 0.]).astype(np.double) dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state()) # R = 100 # Cost on input "effort". u = dircol.input() x = dircol.state() # t = dircol.time() # let's add 100*t (seconds) to get in that min-time component! denom1 = float(10**2 + math.pi**2 + 10**2 + math.pi**2) denom2 = float(180**2) #denom1 = 10**2+math.pi**2+10**2+math.pi**2 #denom2 = 180**2 # dircol.AddRunningCost(u.dot(u)/denom2) # dircol.AddRunningCost(2*(x-final_state).dot(x-final_state)/denom1) dircol.AddRunningCost(1 + 2. * (x - final_state).dot(x - final_state) / denom1 + u.dot(u) / denom2) # Add a final cost equal to the total duration. #dircol.AddFinalCost(dircol.time()) # Enabled to sim min time cost? if warm_start == "linear": initial_u_trajectory = PiecewisePolynomial() initial_x_trajectory = \ PiecewisePolynomial.FirstOrderHold([0., 4.], np.column_stack((initial_state, final_state))) dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory) elif warm_start == "random": assert isinstance(seed, int) np.random.seed(seed) breaks = np.linspace(0, 4, num_samples).reshape( (-1, 1)) # using num_time_samples u_knots = np.random.rand( 1, num_samples) - 0.5 # num_inputs vs num_samples? x_knots = np.random.rand( 2, num_samples) - 0.5 # num_states vs num_samples? initial_u_trajectory = PiecewisePolynomial.Cubic( breaks, u_knots, False) initial_x_trajectory = PiecewisePolynomial.Cubic( breaks, x_knots, False) dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory) elif warm_start == "target": assert target_traj != [], "Need a valid target for warm starting" (breaks, x_knots, u_knots) = target_traj #(breaks, u_knots, x_knots) = target_traj initial_u_trajectory = PiecewisePolynomial.Cubic( breaks.T, u_knots.T, False) initial_x_trajectory = PiecewisePolynomial.Cubic( breaks.T, x_knots.T, False) dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory) def cb(decision_vars): global vis_cb_counter vis_cb_counter += 1 if vis_cb_counter % 10 != 0: return # Get the total cost all_costs = dircol.EvalBindings(dircol.GetAllCosts(), decision_vars) # Get the total cost of the constraints. # Additionally, the number and extent of any constraint violations. violated_constraint_count = 0 violated_constraint_cost = 0 constraint_cost = 0 for constraint in dircol.GetAllConstraints(): val = dircol.EvalBinding(constraint, decision_vars) # Consider switching to DoCheckSatisfied if you can find the binding... nudge = 1e-1 # This much constraint violation is not considered bad... lb = constraint.evaluator().lower_bound() ub = constraint.evaluator().upper_bound() good_lb = np.all(np.less_equal(lb, val + nudge)) good_ub = np.all(np.greater_equal(ub, val - nudge)) if not good_lb or not good_ub: # print("{} <= {} <= {}".format(lb, val, ub)) violated_constraint_count += 1 # violated_constraint_cost += np.sum(np.abs(val)) if not good_lb: violated_constraint_cost += np.sum(np.abs(lb - val)) if not good_ub: violated_constraint_cost += np.sum(np.abs(val - ub)) constraint_cost += np.sum(np.abs(val)) print("total cost: {: .2f} | \tconstraint {: .2f} \tbad {}, {: .2f}". format(sum(all_costs), constraint_cost, violated_constraint_count, violated_constraint_cost)) #dircol.AddVisualizationCallback(cb, dircol.decision_variables()) def MyVisualization(sample_times, values): def state_to_tip_coord(state_vec): # State: (x, theta, x_dot, theta_dot) x, theta, _, _ = state_vec pole_length = 0.5 # manually looked this up #return (x-pole_length*np.sin(theta), pole_length-np.cos(theta)) return (x - pole_length * np.sin(theta), pole_length * (-np.cos(theta))) global vis_cb_counter vis_cb_counter += 1 if vis_cb_counter % 30 != 0: return coords = [state_to_tip_coord(state) for state in values.T] x, y = zip(*coords) plt.plot(x, y, '-o', label=vis_cb_counter) #plt.show() # good? #if should_vis: if False: plt.figure() plt.title('Tip trajectories') plt.xlabel('x') plt.ylabel('x_dot') dircol.AddStateTrajectoryCallback(MyVisualization) from pydrake.all import (SolverType) # dircol.SetSolverOption(SolverType.kSnopt, 'Major feasibility tolerance', 1.0e-6) # default="1.0e-6" # dircol.SetSolverOption(SolverType.kSnopt, 'Major optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1 # dircol.SetSolverOption(SolverType.kSnopt, 'Minor feasibility tolerance', 1.0e-6) # default="1.0e-6" # dircol.SetSolverOption(SolverType.kSnopt, 'Minor optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1 # dircol.SetSolverOption(SolverType.kSnopt, 'Time limit (secs)', 12.0) # default="9999999.0" # Very aggressive cutoff... dircol.SetSolverOption( SolverType.kSnopt, 'Major step limit', 0.1) # default="2.0e+0" # HUGE!!! default takes WAY too huge steps dircol.SetSolverOption(SolverType.kSnopt, 'Time limit (secs)', 15.0) # default="9999999.0" # was 15 # dircol.SetSolverOption(SolverType.kSnopt, 'Reduced Hessian dimension', 10000) # Default="min{2000, n1 + 1}" # dircol.SetSolverOption(SolverType.kSnopt, 'Hessian updates', 30) # Default="10" dircol.SetSolverOption(SolverType.kSnopt, 'Major iterations limit', 9300000) # Default="9300" dircol.SetSolverOption(SolverType.kSnopt, 'Minor iterations limit', 50000) # Default="500" dircol.SetSolverOption(SolverType.kSnopt, 'Iterations limit', 50 * 10000) # Default="10000" # Factoriztion? # dircol.SetSolverOption(SolverType.kSnopt, 'QPSolver Cholesky', True) # Default="*Cholesky/CG/QN" return dircol