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 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 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 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
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
AddFlatTerrainToWorld(tree_1, 100, 10)
world_frame_1 = RigidBodyFrame("world_frame", tree_1.world(), [0, 0, 0], [0, 0, 0])
AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed, world_frame_1, tree_1)
tree_2 = RigidBodyTree()
AddFlatTerrainToWorld(tree_2, 100, 10)
world_frame_2 = RigidBodyFrame("world_frame", tree_1.world(), [0, 0, 0], [0, 0, 0])
AddModelInstanceFromUrdfFile(kuka_urdf_path, FloatingBaseType.kFixed, world_frame_2, tree_2)
builder = DiagramBuilder()
# input trajectory
tspan = [0., TBD]
ts = np.linspace(tspan[0], tspan[-1], TBD)
q_des = TBD
q_traj = PiecewisePolynomial.Cubic(ts, q_des, np.zeros(7), np.zeros(7))
x_init = np.vstack((q_traj.value(tspan[0]),q_traj.derivative(1).value(tspan[0])))
source = builder.AddSystem(TrajectorySource(q_traj, output_derivative_order=1))
# controller
kp = 100 * np.ones(7)
kd = 10 * np.ones(7)
ki = 0 * np.ones(7)
controller = builder.AddSystem(InverseDynamicsController(robot=tree_1, kp=kp, ki=ki, kd=kd, has_reference_acceleration=False))
# plant
plant = RigidBodyPlant(tree_2, 0.0005)
kuka = builder.AddSystem(plant)
# visualizer
lcm = DrakeLcm()