Example #1
0
    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
Example #2
0
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
Example #3
0
    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))
Example #4
0
		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
Example #5
0
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
Example #7
0
 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")
Example #8
0
    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)
Example #9
0
    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)])
Example #10
0
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
Example #11
0
        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
Example #13
0
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
Example #15
0
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
Example #16
0
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
Example #17
0
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
Example #18
0
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)
Example #19
0
# 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:
Example #21
0
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)
Example #23
0
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.]))
Example #24
0
#                               == 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(),
Example #25
0
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
Example #26
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)
Example #27
0
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)
Example #28
0
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
Example #30
0
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