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