double_pendulum.trajectory_optimization.direct_collocation

Submodules

double_pendulum.trajectory_optimization.direct_collocation.dircol_utils

double_pendulum.trajectory_optimization.direct_collocation.dircol_utils.animation(plant, scene_graph, x_trajectory)

Animate a trajectory in browser window.

Parameters:
plantpydrake.multibody.plant.MultibodyPlant

pydrake plant

scene_graphpydrake.geometry.SceneGraph

pydrake scene graph

x_trajectorypydrake.trajectories.PiecewisePolynomial_[float]

state trajectory to be animated

double_pendulum.trajectory_optimization.direct_collocation.dircol_utils.construct_trajectories(dircol, result)

Construct trajectories from direct collocation result.

Parameters:
dircolpydrake.systems.trajectory_optimization.DirectCollocation

pydrake direct collocation object

result
Returns:
pydrake.trajectories.PiecewisePolynomial_[float]

state trajectory

pydrake.trajectories.PiecewisePolynomial_[float]

acceleration trajectory

pydrake.trajectories.PiecewisePolynomial_[float]

jerk trajectory

pydrake.trajectories.PiecewisePolynomial_[float]

torque trajectory

double_pendulum.trajectory_optimization.direct_collocation.dircol_utils.create_plant_from_urdf(urdf_path)

Create a pydrake plant froma urdf file.

Parameters:
urdf_pathstring or path object

path to urdf file

Returns:
pydrake.multibody.plant.MultibodyPlant

pydrake plant

pydrake.systems.Context

pydrake context

pydrake.geometry.SceneGraph

pydrake scene graph

double_pendulum.trajectory_optimization.direct_collocation.dircol_utils.extract_data_from_polynomial(polynomial, frequency)

Extract data points from pydrake polnomial

Parameters:
polynomialpydrake.trajectories.PiecewisePolynomial_[float]

polynomial to be sampled for data points

frequencyfloat

Frequency of the extracted data trajectory

Returns:
numpy_array

shape=(N, 4) state trajectory

numpy_array

shape=(N,) time trajectory

double_pendulum.trajectory_optimization.direct_collocation.direct_collocation_drake

class double_pendulum.trajectory_optimization.direct_collocation.direct_collocation_drake.dircol_calculator(urdf_path, robot, model_pars, save_dir='.')

Bases: object

Class to calculate a trajectory for the double pendulum, acrobot or pendubot with the direct collocation method. Implementation uses drake.

Parameters:
urdf_pathstring or path object

path to urdf file

robotstring
robot which is used, Options:

  • “double_pendulum”

  • “acrobot”

  • “pendubot”

model_parsmodel_parameters object

object of the model_parameters class

save_dirstring

path to directory where log data can be stored (necessary for temporary generated urdf) (Default value=”.”)

animate_trajectory()

Animate the trajectory, found by the optimization, with the drake meshcat viewer in a browser window.

compute_trajectory(n, tau_limit, initial_state, final_state, theta_limit, speed_limit, R, time_penalization, init_traj_time_interval, minimum_timestep, maximum_timestep)

compute_trajectory.

Parameters:
nint

number of knot points for the trajectory

tau_limitfloat

torque limit, unit=[Nm]

initial_statearray_like

shape=(4,) initial_state for the trajectory

final_statearray_like

shape=(4,) final_state for the trajectory

theta_limitfloat

position limit

speed_limitfloat

velocity limit

Rfloat

control/motor torque cost

time_penalizationfloat

cost for trajectory length

init_traj_time_intervallist

shape=(2,) initial time interval for trajectory

minimum_timestepfloat

minimum timestep size, unit=[s]

maximum_timestepfloat

maximum timestep size, unit=[s]

Raises:
AssertionError

If the optmization is not successful

get_trajectory(freq)

Get the trajectory found by the optimization.

Parameters:
freqfloat

frequency with which the trajectory is sampled

Returns:
numpy_array

time points, unit=[s] shape=(N,)

numpy_array

shape=(N, 4) states, units=[rad, rad, rad/s, rad/s] order=[angle1, angle2, velocity1, velocity2]

numpy_array

shape=(N, 2) actuations/motor torques order=[u1, u2], units=[Nm]

double_pendulum.trajectory_optimization.direct_collocation.direct_collocation_drake2

class double_pendulum.trajectory_optimization.direct_collocation.direct_collocation_drake2.DirCol(urdfPath, RobotType, modelPars, saveDir='.', nx=4, nu=2)

Bases: object

This class formulates and solves the mathematical program for trajectory optimization via direct collocation. It uses drake for parsing the plant from a urdf as well as formulating and solving the optimization problem. The drake-internal direct collocation class is avoided for better control over the problem formulation. The class also implements resampling of the found trajectory

CollocationConstraint(vars)

Helper function to compute collocation constraint. Adds support for calls of plant within constraints

ComputeTrajectory(freq)

Resamples trajectories using scipy CubicHermiteSpline for state trajectory and 1st order hold for input trajectory

Parameters:

freq (float64) – sampling frequency

Returns:

time vector x (array): resampled state trajectory u (array): resampled input trajectory

Return type:

t (array)

EquationOfMotion(x, u, plant, context)

Computes the plant dynamics based on current state and input. Uses drake to extract matrices

Parameters:

  • x (array) – current state. x.shape=(nx,)

  • u (array) – current input. u.shape=(nu,)

  • plant (pydrake.plant) – plant

  • context (pydrake.context) – plant context

Returns:

system dynamics xdot=(qd,qdd)

Return type:

array

InterpolationConstraint(vars)

Helper function to compute interpolation constraint. Adds support for calls of plant within constraints

MathematicalProgram(N, Q, R, wh, h_min, h_max, x0, xf, torque_limit, X_initial, U_initial, h_initial)

Constructs the mathematical program and solves it using SNOPT from drake. Problem formulation done as described in paper by Matthew Kelly(2017), DOI:10.1137/16M1062569 Stores solutions as class variables

Parameters:

  • N (int) – Number of knot points

  • Q (array) – weighting matrix of state

  • R (_type_) – weighting matrix of input

  • wh (_type_) – weighting factor of time step length

  • h_min (_type_) – lower bound time step length

  • h_max (_type_) – upper bound time step length

  • x0 (_type_) – initial state

  • xf (_type_) – final state

  • torque_limit (_type_) – lower and upper bound input torque [Nm]

  • X_initial (_type_) – initial guess state trajectory

  • U_initial (_type_) – initial guess control trajectory

  • h_initial (_type_) – initial guess time step length