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 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 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 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 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 _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 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 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 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 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 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 run_benchmark( zmq_url=None, P_WORLD_TARGET=np.array([-1, 1, 0]), MAX_APPROACH_ANGLE=-45 / 180.0 * np.pi, OBJECT_TO_TOSS="ball", GRIPPER_TO_OBJECT_COM_DIST=0.11, LAUNCH_ANGLE_THRESH=3 / 180.0 * np.pi, # 3 seems to work well? verbose=False, **kwargs, ): # Initialize global plants ONCE for IK calculations # This speed up exectuation greatly. GLOBAL_PLANT, GLOBAL_CONTEXT = get_basic_manip_station() """ https://schunk.com/fileadmin/pim/docs/IM0026091.PDF - gripper frame to tip is about .115 m for reference GRIPPER_TO_OBJECT_FRAME_DIST = 0.12 # meters, this is how much "above" the balls origin we must send the gripper body frame in order to successfully grasp the object OBJECT_FRAME_TO_COM_DIST = 0.05 / 2 """ T_world_target = RigidTransform(RotationMatrix(), P_WORLD_TARGET) T_world_objectInitial = RigidTransform( #p=[-.1, -.69, 1.04998503e-01], # sphere p=[-0.1, -0.69, 0.09], # foam_brick R=RotationMatrix.MakeZRotation(np.pi / 2.0)) T_world_gripperObject = RigidTransform( p=T_world_objectInitial.translation() + np.array([0, 0, 0.025 + GRIPPER_TO_OBJECT_COM_DIST]), R=RotationMatrix.MakeXRotation(-np.pi / 2.0)) T_world_objectCOM = RigidTransform( T_world_objectInitial.rotation(), T_world_objectInitial.translation() + np.array([0, 0, 0.025])) # Set starting and ending joint angles for throw throw_heading = np.arctan2(P_WORLD_TARGET[1], P_WORLD_TARGET[0]) ja1 = throw_heading - np.pi # TODO: edit these to work better for large angles. PRETHROW_JA = np.array([ja1, 0, 0, 1.9, 0, -1.9, 0, 0, 0]) THROWEND_JA = np.array([ja1, 0, 0, 0.4, 0, -0.4, 0, 0, 0]) T_world_prethrowPose = jointangles_to_pose( plant=GLOBAL_PLANT, context=GLOBAL_CONTEXT, jointangles=PRETHROW_JA[:7], ) T_world_robotInitial, meshcat = setup_manipulation_station( T_world_objectInitial, zmq_url, T_world_target, manipuland=OBJECT_TO_TOSS) #object frame viz if meshcat: visualize_transform(meshcat, "T_world_obj0", T_world_objectInitial) visualize_transform(meshcat, "T_world_objectCOM", T_world_objectCOM) visualize_transform(meshcat, "T_world_gripperObject", T_world_gripperObject) T_world_target = RigidTransform(p=P_WORLD_TARGET, R=RotationMatrix()) visualize_transform(meshcat, "target", T_world_target) def throw_objective(inp, g=9.81, alpha=1, return_other=None): throw_motion_time, release_frac = inp release_ja = PRETHROW_JA + release_frac * (THROWEND_JA - PRETHROW_JA) T_world_releasePose = jointangles_to_pose(plant=GLOBAL_PLANT, context=GLOBAL_CONTEXT, jointangles=release_ja[:7]) p_release = (T_world_releasePose.translation() + T_world_releasePose.rotation().multiply( [0, GRIPPER_TO_OBJECT_COM_DIST, 0])) J_release = spatial_velocity_jacobian_at_jointangles( plant=GLOBAL_PLANT, context=GLOBAL_CONTEXT, jointangles=release_ja[:7], gripper_to_object_dist=GRIPPER_TO_OBJECT_COM_DIST # <==== important )[3:6] v_release = J_release @ ( (THROWEND_JA - PRETHROW_JA) / throw_motion_time)[:7] if v_release[:2] @ (P_WORLD_TARGET - p_release)[:2] <= 0: return 1000 x = np.linalg.norm((P_WORLD_TARGET - p_release)[:2]) y = (P_WORLD_TARGET - p_release)[2] vx = np.linalg.norm(v_release[:2]) vy = v_release[2] tta = x / vx y_hat = vy * tta - 0.5 * g * tta**2 phi_hat = np.arctan((vy - g * tta) / vx) objective = ((y_hat - y)**2 + np.maximum(phi_hat - MAX_APPROACH_ANGLE, 0)**2 #+ (phi_hat - MAX_APPROACH_ANGLE) ** 2 ) if objective < 1e-6: # if we hit target at correct angle # then try to move as slow as possible objective -= throw_motion_time**2 if return_other == "launch": return np.arctan2(vy, vx) elif return_other == "land": return phi_hat elif return_other == "tta": return tta else: return objective res = scipy.optimize.differential_evolution(throw_objective, bounds=[(1e-3, 3), (0.1, 0.9)], seed=43) throw_motion_time, release_frac = res.x assert throw_motion_time > 0 assert 0 < release_frac < 1 plan_launch_angle = throw_objective(res.x, return_other="launch") plan_land_angle = throw_objective(res.x, return_other="land") tta = throw_objective(res.x, return_other="tta") if verbose: print(f"Throw motion will take: {throw_motion_time:.4f} seconds") print(f"Releasing at {release_frac:.4f} along the motion") print(f"Launch angle (degrees): {plan_launch_angle / np.pi * 180.0}") print(f"Plan land angle (degrees): {plan_land_angle / np.pi * 180.0}") print(f"time to arrival: {tta}") ''' timings ''' t_goToObj = 1.0 t_holdObj = 2.0 t_goToPreobj = 1.0 t_goToWaypoint = 2.0 t_goToPrethrow = 4.0 # must be greater than 1.0 for a 1 second hold to stabilize t_goToThrowEnd = throw_motion_time # plan pickup t_lst, q_knots, total_time = plan_pickup(T_world_robotInitial, T_world_gripperObject, t_goToObj=t_goToObj, t_holdObj=t_holdObj, t_goToPreobj=t_goToPreobj) # clear the bins via a waypoint 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), ) t_lst, q_knots = add_go_to_pose_via_jointinterpolation( T_world_robotInitial, T_world_hackyWayPoint, t_start=total_time, t_lst=t_lst, q_knots=q_knots, time_interval_s=t_goToWaypoint) # go to prethrow t_lst, q_knots = add_go_to_ja_via_jointinterpolation( pose_to_jointangles(T_world_hackyWayPoint), PRETHROW_JA, t_start=total_time + t_goToWaypoint, t_lst=t_lst, q_knots=q_knots, time_interval_s=t_goToPrethrow, hold_time_s=1.0, ) # go to throw t_lst, q_knots = add_go_to_ja_via_jointinterpolation( PRETHROW_JA, THROWEND_JA, t_start=total_time + t_goToWaypoint + t_goToPrethrow, t_lst=t_lst, q_knots=q_knots, time_interval_s=t_goToThrowEnd, num_samples=30, include_end=True) # turn trajectory into joint space q_knots = np.array(q_knots) q_traj = PiecewisePolynomial.CubicShapePreserving(t_lst, q_knots[:, 0:7].T) ''' Gripper reference trajectory (not used, we use a closed loop controller instead) ''' # make gripper trajectory assert t_holdObj > 1.5 gripper_times_lst = np.array([ 0., t_goToObj, 1.0, # pickup object 0.25, # pickup object t_holdObj - 1.25, # pickup object t_goToPreobj, t_goToWaypoint, t_goToPrethrow, release_frac * t_goToThrowEnd, 1e-9, (1 - release_frac) * t_goToThrowEnd, ]) 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_OPEN, # pickup object GRIPPER_CLOSED, # pickup object GRIPPER_CLOSED, # pickup object GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_CLOSED, GRIPPER_OPEN, GRIPPER_OPEN, ]).reshape(1, gripper_times_lst.shape[0]) g_traj = PiecewisePolynomial.FirstOrderHold(gripper_cumulative_times_lst, gripper_knots) get_gripper_controller_3 = lambda station_plant: GripperControllerUsingIiwaStateV3( plant=station_plant, gripper_to_object_dist=GRIPPER_TO_OBJECT_COM_DIST, T_world_objectPickup=T_world_gripperObject, T_world_prethrow=T_world_prethrowPose, planned_launch_angle=plan_launch_angle, launch_angle_thresh=LAUNCH_ANGLE_THRESH, dbg_state_prints=verbose) # do the thing simulator, _, meshcat, loggers = BuildAndSimulateTrajectory( q_traj=q_traj, g_traj=g_traj, get_gripper_controller=get_gripper_controller_3, T_world_objectInitial= T_world_objectInitial, # where to init the object in the world T_world_targetBin= T_world_target, # where the ball should hit - aka where the bin will catch it zmq_url=zmq_url, time_step= 1e-3, # target (-6, 6, -1). 1e-3 => overshoot barely, 1e-4 => undershoot barely, look around 7.92-7.94 s in sim include_target_bin=False, manipuland=OBJECT_TO_TOSS) fly_time = max(tta + 1, 2) if verbose: print( f"Throw motion should happen from 9.5 seconds to {10 + throw_motion_time} seconds" ) print(f"Running for {q_traj.end_time() + fly_time} seconds") if meshcat: meshcat.start_recording() simulator.AdvanceTo(q_traj.end_time() + fly_time) if meshcat: meshcat.stop_recording() meshcat.publish_recording() all_log_times = loggers["state"].sample_times() chosen_idxs = (9 < all_log_times) & (all_log_times < 100) log_times = loggers["state"].sample_times()[chosen_idxs] log_states = loggers["state"].data()[:, chosen_idxs] p_objects = np.zeros((len(log_times), 3)) p_objects[:, 0] = log_states[4] p_objects[:, 1] = log_states[5] p_objects[:, 2] = log_states[6] deltas = p_objects - P_WORLD_TARGET land_idx = np.where(deltas[:, 2] > 0)[0][-1] p_land = p_objects[land_idx] is_overthrow = np.linalg.norm(p_land[:2]) > np.linalg.norm( P_WORLD_TARGET[:2]) delta_land = deltas[land_idx] land_pos_error = np.linalg.norm(delta_land) * (1 if is_overthrow else -1) aim_angle_error = (np.arctan2(p_land[1], p_land[0]) - np.arctan2(P_WORLD_TARGET[1], P_WORLD_TARGET[0])) v_land = ((p_objects[land_idx] - p_objects[land_idx - 1]) / (log_times[land_idx] - log_times[land_idx - 1])) sim_land_angle = np.arctan(v_land[2] / np.linalg.norm(v_land[:2])) land_angle_error = sim_land_angle - plan_land_angle ret_dict = dict( land_pos_error=land_pos_error, land_angle_error=land_angle_error, aim_angle_error=aim_angle_error, time_to_arrival=tta, land_time=log_times[land_idx], land_x=p_land[0], land_y=p_land[1], land_z=p_land[2], sim_land_angle=sim_land_angle, plan_land_angle=plan_land_angle, plan_launch_angle=plan_launch_angle, release_frac=release_frac, throw_motion_time=throw_motion_time, ) return ret_dict
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
i = 0 while i < len(t_impacts): if i == 0: # Two quads and one ball t_init = 0 state_init_i = copy.copy(state_init) t_impacts_i = np.concatenate((t_impacts, np.array([T_total]))) impact_combination_i = copy.copy(impact_combination) T_i = T_total # Set up initial guesses quad_plant = Quadrotor2D() h_init = h_max pos_indices = getPosIndices() q_init_poly = PiecewisePolynomial.FirstOrderHold( [0, T_i * h_init], np.column_stack( (state_init[pos_indices], state_final[pos_indices]))) qd_init_poly = q_init_poly.derivative() u_init_poly = PiecewisePolynomial.ZeroOrderHold( [0, T_i * h_init], 0.5 * quad_plant.mass * quad_plant.gravity * np.ones( (2 * n_quadrotors, 2))) u_init = np.hstack( [u_init_poly.value(t * h_init) for t in range(T_i + 1)]).T q_init = np.hstack( [q_init_poly.value(t * h_init) for t in range(T_i + 1)]).T qd_init = np.hstack( [qd_init_poly.value(t * h_init) for t in range(T_i + 1)]).T x_init = collatePosAndVel(q_init, qd_init)
# 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)
dircol.AddBoundingBoxConstraint(initial_state, initial_state, dircol.initial_state()) # 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))) # yapf: disable 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) builder = DiagramBuilder() source = builder.AddSystem(TrajectorySource(x_trajectory)) scene_graph = builder.AddSystem(SceneGraph()) AcrobotGeometry.AddToBuilder(builder, source.get_output_port(0), scene_graph) visualizer = builder.AddSystem( PlanarSceneGraphVisualizer(scene_graph, xlim=[-4., 4.], ylim=[-4., 4.]))
# == 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 solveOptimization(state_init, t_impact, impact_combination, T, u_guess=None, x_guess=None, h_guess=None): prog = MathematicalProgram() h = prog.NewContinuousVariables(T, name='h') u = prog.NewContinuousVariables(rows=T + 1, cols=2 * n_quadrotors, name='u') x = prog.NewContinuousVariables(rows=T + 1, cols=6 * n_quadrotors + 4 * n_balls, name='x') dv = prog.decision_variables() prog.AddBoundingBoxConstraint([h_min] * T, [h_max] * T, h) for i in range(n_quadrotors): sys = Quadrotor2D() context = sys.CreateDefaultContext() dir_coll_constr = DirectCollocationConstraint(sys, context) ind_x = 6 * i ind_u = 2 * i for t in range(T): impact_indices = impact_combination[np.argmax( np.abs(t - t_impact) <= 1)] quad_ind, ball_ind = impact_indices[0], impact_indices[1] if quad_ind == i and np.any( t == t_impact ): # Don't add Direct collocation constraint at impact continue elif quad_ind == i and (np.any(t == t_impact - 1) or np.any(t == t_impact + 1)): prog.AddConstraint( eq( x[t + 1, ind_x:ind_x + 3], x[t, ind_x:ind_x + 3] + h[t] * x[t + 1, ind_x + 3:ind_x + 6])) # Backward euler prog.AddConstraint( eq(x[t + 1, ind_x + 3:ind_x + 6], x[t, ind_x + 3:ind_x + 6]) ) # Zero-acceleration assumption during this time step. Should maybe replace with something less naive else: AddDirectCollocationConstraint( dir_coll_constr, np.array([[h[t]]]), x[t, ind_x:ind_x + 6].reshape(-1, 1), x[t + 1, ind_x:ind_x + 6].reshape(-1, 1), u[t, ind_u:ind_u + 2].reshape(-1, 1), u[t + 1, ind_u:ind_u + 2].reshape(-1, 1), prog) for i in range(n_balls): sys = Ball2D() context = sys.CreateDefaultContext() dir_coll_constr = DirectCollocationConstraint(sys, context) ind_x = 6 * n_quadrotors + 4 * i for t in range(T): impact_indices = impact_combination[np.argmax( np.abs(t - t_impact) <= 1)] quad_ind, ball_ind = impact_indices[0], impact_indices[1] if ball_ind == i and np.any( t == t_impact ): # Don't add Direct collocation constraint at impact continue elif ball_ind == i and (np.any(t == t_impact - 1) or np.any(t == t_impact + 1)): prog.AddConstraint( eq( x[t + 1, ind_x:ind_x + 2], x[t, ind_x:ind_x + 2] + h[t] * x[t + 1, ind_x + 2:ind_x + 4])) # Backward euler prog.AddConstraint( eq(x[t + 1, ind_x + 2:ind_x + 4], x[t, ind_x + 2:ind_x + 4] + h[t] * np.array([0, -9.81]))) else: AddDirectCollocationConstraint( dir_coll_constr, np.array([[h[t]]]), x[t, ind_x:ind_x + 4].reshape(-1, 1), x[t + 1, ind_x:ind_x + 4].reshape(-1, 1), u[t, 0:0].reshape(-1, 1), u[t + 1, 0:0].reshape(-1, 1), prog) # Initial conditions prog.AddLinearConstraint(eq(x[0, :], state_init)) # Final conditions prog.AddLinearConstraint(eq(x[T, 0:14], state_final[0:14])) # Quadrotor final conditions (full state) for i in range(n_quadrotors): ind = 6 * i prog.AddLinearConstraint( eq(x[T, ind:ind + 6], state_final[ind:ind + 6])) # Ball final conditions (position only) for i in range(n_balls): ind = 6 * n_quadrotors + 4 * i prog.AddLinearConstraint( eq(x[T, ind:ind + 2], state_final[ind:ind + 2])) # Input constraints for i in range(n_quadrotors): prog.AddLinearConstraint(ge(u[:, 2 * i], -20.0)) prog.AddLinearConstraint(le(u[:, 2 * i], 20.0)) prog.AddLinearConstraint(ge(u[:, 2 * i + 1], -20.0)) prog.AddLinearConstraint(le(u[:, 2 * i + 1], 20.0)) # Don't allow quadrotor to pitch more than 60 degrees for i in range(n_quadrotors): prog.AddLinearConstraint(ge(x[:, 6 * i + 2], -np.pi / 3)) prog.AddLinearConstraint(le(x[:, 6 * i + 2], np.pi / 3)) # Ball position constraints # for i in range(n_balls): # ind_i = 6*n_quadrotors + 4*i # prog.AddLinearConstraint(ge(x[:,ind_i],-2.0)) # prog.AddLinearConstraint(le(x[:,ind_i], 2.0)) # prog.AddLinearConstraint(ge(x[:,ind_i+1],-3.0)) # prog.AddLinearConstraint(le(x[:,ind_i+1], 3.0)) # Impact constraint quad_temp = Quadrotor2D() for i in range(n_quadrotors): for j in range(n_balls): ind_q = 6 * i ind_b = 6 * n_quadrotors + 4 * j for t in range(T): if np.any( t == t_impact ): # If quad i and ball j impact at time t, add impact constraint impact_indices = impact_combination[np.argmax( t == t_impact)] quad_ind, ball_ind = impact_indices[0], impact_indices[1] if quad_ind == i and ball_ind == j: # At impact, witness function == 0 prog.AddConstraint(lambda a: np.array([ CalcClosestDistanceQuadBall(a[0:3], a[3:5]) ]).reshape(1, 1), lb=np.zeros((1, 1)), ub=np.zeros((1, 1)), vars=np.concatenate( (x[t, ind_q:ind_q + 3], x[t, ind_b:ind_b + 2])).reshape( -1, 1)) # At impact, enforce discrete collision update for both ball and quadrotor prog.AddConstraint( CalcPostCollisionStateQuadBallResidual, lb=np.zeros((6, 1)), ub=np.zeros((6, 1)), vars=np.concatenate( (x[t, ind_q:ind_q + 6], x[t, ind_b:ind_b + 4], x[t + 1, ind_q:ind_q + 6])).reshape(-1, 1)) prog.AddConstraint( CalcPostCollisionStateBallQuadResidual, lb=np.zeros((4, 1)), ub=np.zeros((4, 1)), vars=np.concatenate( (x[t, ind_q:ind_q + 6], x[t, ind_b:ind_b + 4], x[t + 1, ind_b:ind_b + 4])).reshape(-1, 1)) # rough constraints to enforce hitting center-ish of paddle prog.AddLinearConstraint( x[t, ind_q] - x[t, ind_b] >= -0.01) prog.AddLinearConstraint( x[t, ind_q] - x[t, ind_b] <= 0.01) continue # Everywhere else, witness function must be > 0 prog.AddConstraint(lambda a: np.array([ CalcClosestDistanceQuadBall(a[ind_q:ind_q + 3], a[ ind_b:ind_b + 2]) ]).reshape(1, 1), lb=np.zeros((1, 1)), ub=np.inf * np.ones((1, 1)), vars=x[t, :].reshape(-1, 1)) # Don't allow quadrotor collisions # for t in range(T): # for i in range(n_quadrotors): # for j in range(i+1, n_quadrotors): # prog.AddConstraint((x[t,6*i]-x[t,6*j])**2 + (x[t,6*i+1]-x[t,6*j+1])**2 >= 0.65**2) # Quadrotors stay on their own side # prog.AddLinearConstraint(ge(x[:, 0], 0.3)) # prog.AddLinearConstraint(le(x[:, 6], -0.3)) ############################################################################### # Set up initial guesses initial_guess = np.empty(prog.num_vars()) # # initial guess for the time step prog.SetDecisionVariableValueInVector(h, h_guess, initial_guess) x_init[0, :] = state_init prog.SetDecisionVariableValueInVector(x, x_guess, initial_guess) prog.SetDecisionVariableValueInVector(u, u_guess, initial_guess) solver = SnoptSolver() print("Solving...") result = solver.Solve(prog, initial_guess) # print(GetInfeasibleConstraints(prog,result)) # be sure that the solution is optimal assert result.is_success() print(f'Solution found? {result.is_success()}.') ################################################################################# # Extract results # get optimal solution h_opt = result.GetSolution(h) x_opt = result.GetSolution(x) u_opt = result.GetSolution(u) time_breaks_opt = np.array([sum(h_opt[:t]) for t in range(T + 1)]) u_opt_poly = PiecewisePolynomial.ZeroOrderHold(time_breaks_opt, u_opt.T) # x_opt_poly = PiecewisePolynomial.Cubic(time_breaks_opt, x_opt.T, False) x_opt_poly = PiecewisePolynomial.FirstOrderHold( time_breaks_opt, x_opt.T ) # Switch to first order hold instead of cubic because cubic was taking too long to create ################################################################################# # Create list of K matrices for time varying LQR context = quad_plant.CreateDefaultContext() breaks = copy.copy( time_breaks_opt) #np.linspace(0, x_opt_poly.end_time(), 100) K_samples = np.zeros((breaks.size, 12 * n_quadrotors)) for i in range(n_quadrotors): K = None for j in range(breaks.size): context.SetContinuousState( x_opt_poly.value(breaks[j])[6 * i:6 * (i + 1)]) context.FixInputPort( 0, u_opt_poly.value(breaks[j])[2 * i:2 * (i + 1)]) linear_system = FirstOrderTaylorApproximation(quad_plant, context) A = linear_system.A() B = linear_system.B() try: K, _, _ = control.lqr(A, B, Q, R) except: assert K is not None, "Failed to calculate initial K for quadrotor " + str( i) print("Warning: Failed to calculate K at timestep", j, "for quadrotor", i, ". Using K from previous timestep") K_samples[j, 12 * i:12 * (i + 1)] = K.reshape(-1) K_poly = PiecewisePolynomial.ZeroOrderHold(breaks, K_samples.T) return u_opt_poly, x_opt_poly, K_poly, h_opt
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