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