pyrieef.motion package

Submodules

pyrieef.motion.control module

class pyrieef.motion.control.KinematicTrajectoryFollowingLQR(dt, trajectory)

Bases: object

T()
dt()
integrate(q_init, start_time_step=0)

Integrate the policy forward in time

q_init : ndarray

policy(t, x_t)

Computes the desired acceleration given an LQR error term gain.

u = -K (x_t - x_dt) + u_d

where x_d and u_d are the state and control along the trajectory

t : float x_t : ndarray

solve_ricatti(Q_p, Q_v, R_a)

State and action are pulerly kinematic and defined as :

q_t : configuration v_t = dot q_t : velocity a_t = ddot q_t : acceleration

x_t = [q_t, v_t] u_t = a_t

the dynamical system is described as:

q_{t+1} = q_t + v_t * dt + a_t * dt^2 v_{t+1} = v_t + a_t * dt

the policy should be:

a_{t+1} = -K (x_t - x_td) + a_td

where x_td and a_td are the state and acceleration along the trajectory.

Q_p : float, position tracking gain Q_v : float, velocity tracking gain R_a : float, control cost

pyrieef.motion.control.controller_lqr_discrete_time(A, B, Q, R)

Solve the constdiscrete time LQR controller for a discrete time constant system.

A and B are system matrices, describing the systems dynamics:

x_{t+1} = A x_t + B u_t

The controller minimizes the infinite horizon quadratic cost function:

cost = sum x_t^T Q x_t + u_t^T R u_t

where Q is a positive semidefinite matrix, and R is positive definite matrix.

A, B, Q, R : numpy matrices

K : gain the optimal gain K, P : the solution matrix X, and the closed loop system eigVals: eigenvalues.

The optimal input is then computed as:

input: u = -K*x

pyrieef.motion.cost_terms module

class pyrieef.motion.cost_terms.BoundBarrier(v_lower, v_upper, margin=1e-10, alpha=1.0)

Bases: geometry.differentiable_geometry.DifferentiableMap

Barrier between values v_lower and v_upper

forward(x)

Should return an array or single value

hessian(x)
Should return the hessian matrix

n x n : input x input (dimensions)

by default the method returns the finite difference hessian that relies on the jacobian function. This method would be a third order tensor in the case of multiple output, we exclude this case for now. WARNING the object returned by this function is a numpy matrix.

input_dimension()
jacobian(x)
Should return a matrix or single value of

m x n : ouput x input (dimensions)

by default the method returns the finite difference jacobian. WARNING the object returned by this function is a numpy matrix.

output_dimension()
class pyrieef.motion.cost_terms.CostGridPotential2D(signed_distance_field, alpha, margin, offset)

Bases: pyrieef.motion.cost_terms.SimplePotential2D

obstacle potential class with margin and offset

forward(x)

Should return an array or single value

class pyrieef.motion.cost_terms.FiniteDifferencesAcceleration(dim, dt)

Bases: geometry.differentiable_geometry.AffineMap

Define accelerations where clique = [ x_{t-1} ; x_{t} ; x_{t+1} ]

class pyrieef.motion.cost_terms.FiniteDifferencesVelocity(dim, dt)

Bases: geometry.differentiable_geometry.AffineMap

Define velocities where clique = [ x_t ; x_{t+1} ]

class pyrieef.motion.cost_terms.LogBarrierFunction(margin=1e-10)

Bases: geometry.differentiable_geometry.DifferentiableMap

Log barrier function

f(x) = -mu log(x)

Note fot the sdf the sign has to be flipped, you can set alpha to -1.

g : R^n -> R, constraint function that allways has to be positive mu : float alpha : float

forward(x)

TODO add this notion of infity

hessian(x)
Should return the hessian matrix

n x n : input x input (dimensions)

by default the method returns the finite difference hessian that relies on the jacobian function. This method would be a third order tensor in the case of multiple output, we exclude this case for now. WARNING the object returned by this function is a numpy matrix.

input_dimension()
jacobian(x)
Should return a matrix or single value of

m x n : ouput x input (dimensions)

by default the method returns the finite difference jacobian. WARNING the object returned by this function is a numpy matrix.

output_dimension()
set_mu(mu)
class pyrieef.motion.cost_terms.ObstaclePotential2D(signed_distance_field, scaling=50.0, alpha=0.001)

Bases: geometry.differentiable_geometry.DifferentiableMap

obstacle potential class

forward(x)

Should return an array or single value

hessian(x)
Should return the hessian matrix

n x n : input x input (dimensions)

by default the method returns the finite difference hessian that relies on the jacobian function. This method would be a third order tensor in the case of multiple output, we exclude this case for now. WARNING the object returned by this function is a numpy matrix.

input_dimension()
jacobian(x)
Should return a matrix or single value of

m x n : ouput x input (dimensions)

by default the method returns the finite difference jacobian. WARNING the object returned by this function is a numpy matrix.

output_dimension()
class pyrieef.motion.cost_terms.SimplePotential2D(signed_distance_field)

Bases: geometry.differentiable_geometry.DifferentiableMap

obstacle potential class

forward(x)

Should return an array or single value

hessian(x)
Should return the hessian matrix

n x n : input x input (dimensions)

by default the method returns the finite difference hessian that relies on the jacobian function. This method would be a third order tensor in the case of multiple output, we exclude this case for now. WARNING the object returned by this function is a numpy matrix.

input_dimension()
jacobian(x)
Should return a matrix or single value of

m x n : ouput x input (dimensions)

by default the method returns the finite difference jacobian. WARNING the object returned by this function is a numpy matrix.

output_dimension()
class pyrieef.motion.cost_terms.SquaredNormAcceleration(dim, dt)

Bases: pyrieef.motion.cost_terms.SquaredNormDerivative

Defines SN of acceleration clique = [x_{t-1} ; x_{t} ; x_{t+1} ]

class pyrieef.motion.cost_terms.SquaredNormDerivative(dim)

Bases: geometry.differentiable_geometry.DifferentiableMap

Define any norm of derivatives clique = [x_t ; x_{t+1} ; … ]

forward(clique)

Should return an array or single value

hessian(clique)
Should return the hessian matrix

n x n : input x input (dimensions)

by default the method returns the finite difference hessian that relies on the jacobian function. This method would be a third order tensor in the case of multiple output, we exclude this case for now. WARNING the object returned by this function is a numpy matrix.

input_dimension()
jacobian(clique)
Should return a matrix or single value of

m x n : ouput x input (dimensions)

by default the method returns the finite difference jacobian. WARNING the object returned by this function is a numpy matrix.

output_dimension()
class pyrieef.motion.cost_terms.SquaredNormVelocity(dim, dt)

Bases: pyrieef.motion.cost_terms.SquaredNormDerivative

Defines SN of velocities where clique = [x_t ; x_{t+1} ]

pyrieef.motion.freeflyer module

class pyrieef.motion.freeflyer.FreeflyerObjective(T=30, n=3, q_init=None, q_goal=None, embedding=None, robot=None)

Bases: object

add_smoothness_terms()
add_terminal_term()
create_clique_network()

pyrieef.motion.geodesic module

class pyrieef.motion.geodesic.GeodesicObjective2D(T=10, n=2, q_init=None, q_goal=None, embedding=None)

Bases: object

add_smoothness_terms()
add_terminal_term()
create_clique_network()

pyrieef.motion.ilqr module

class pyrieef.motion.ilqr.IterativeLQR(clique_hessians, clique_jacobians, T_length, dt)

Bases: object

backward_pass()
forward_pass(start_point, x_d, u_d)

pyrieef.motion.objective module

class pyrieef.motion.objective.MotionOptimization2DCostMap(T=10, n=2, box=<geometry.workspace.EnvBox object>, signed_distance_field=None, costmap=None, q_init=None, q_goal=None)

Bases: object

add_all_terms()
add_attractor(trajectory)

Add an attractor to each clique scalled by the distance to the goal, it ensures that the trajectory does not slow down in time as it progresses towards the goal. This is Model Predictive Control grounded scheme. TODO check the literature to set this appropriatly.

add_box_limits()
add_costgrid_terms(scalar=1.0)

Takes a matrix and adds a isometric potential term to all cliques

add_final_velocity_terms()
add_init_and_terminal_terms()
add_isometric_potential_to_all_cliques(potential, scalar)

Apply the following euqation to all cliques:

c(x_t) | d/dt x_t |

The resulting Riemanian metric is isometric. TODO see paper. Introduced in CHOMP, Ratliff et al. 2009.

add_obstacle_barrier()

obstacle barrier function

add_obstacle_terms(geodesic=False)

Takes a matrix and adds an isometric potential term to all cliques

add_smoothness_terms(deriv_order=2)
add_waypoint_terms(q_waypoint, i, scalar)
cost(trajectory)

compute sum of acceleration

create_clique_network()
create_objective()

resets the objective

create_sdf_hardcoded_workspace()
create_sdf_test_workspace()
create_smoothness_metric()

TODO this does not seem to work at all…

obstacle_potential_from_sdf()
optimize(q_init, nb_steps=100, trajectory=None, optimizer='natural_gradient')
set_eta(eta)
set_problem(workspace, trajectory, obstacle_potential)
set_scalars(obstacle_scalar=1.0, init_potential_scalar=0.0, term_potential_scalar=10000000.0, velocity_scalar=1.0, acceleration_scalar=1)
set_test_objective()

This objective does not collide with the enviroment

pyrieef.motion.objective.smoothness_metric(dt, T, n)

TODO this does not seem to work at all…

pyrieef.motion.trajectory module

class pyrieef.motion.trajectory.CliquesFunctionNetwork(input_dimension, clique_element_dim)

Bases: pyrieef.motion.trajectory.FunctionNetwork

Base class to implement a function network It allows to register functions and evaluates f(x_{i-1}, x_i, x_{i+1}) = sum_i f_i(x_{i-1}, x_i, x_{i+1})

all_cliques(x)

returns a list of all cliques

center_of_clique_map()

x_{t}

clique_hessian(H, t)

return the clique hessian H : the full hessian

clique_jacobian(J, t)

return the clique jacobian J : the full jacobian

clique_value(t, x_t)

return the clique value TODO create a test using this function.

forward(x)

We call over all subfunctions in each clique

hessian(x)
The hessian matrix is of dimension m x m

m (rows) : input size m (cols) : input size

input_dimension()
jacobian(x)
The jacboian matrix is of dimension m x n

m (rows) : output size n (cols) : input size

which can also be viewed becase the first order Taylor expansion of any differentiable map is f(x) = f(x_0) + J(x_0)_f x, where x is a collumn vector. The sub jacobian of the maps are the sum of clique jacobians each clique function f : R^dim -> R, where dim is the clique size.

left_most_of_clique_map()

x_{t-1}

left_of_clique_map()

x_{t-1} ; x_{t}

nb_cliques()
output_dimension()
register_function_for_all_cliques(f)

Register function f

register_function_for_clique(t, f)

Register function f for clique i

register_function_last_clique(f)

Register function f

right_most_of_clique_map()

x_{t+1}

right_of_clique_map()

x_{t} ; x_{t+1}

class pyrieef.motion.trajectory.ConstantAccelerationTrajectory(T=0, n=2, dt=0.1, q_init=None, x=None)

Bases: pyrieef.motion.trajectory.ContinuousTrajectory

Implements a trajectory that can be continously interpolated

config_at_time(t)

Get the id of the segment and then interpolate using quadric interpolation

class pyrieef.motion.trajectory.ContinuousTrajectory(T=0, n=2, q_init=None, x=None)

Bases: pyrieef.motion.trajectory.Trajectory

Implements a trajectory that can be continously interpolated

configuration_at_parameter(s)

The trajectory is indexed by s in [0, 1]

length()

length in configuration space

class pyrieef.motion.trajectory.CubicSplineTrajectory(T=0, n=2, dt=0.1, q_init=None, x=None)

Bases: pyrieef.motion.trajectory.ContinuousTrajectory

Implements a trajectory that can be continously interpolated

Velocities and accelerations are computed using finite differences

which is why we set an epsilon parameter.

The trajectory is defined for a time interval

t in [0., dt * (T + 1)]

A trajectory is always defined from index 0 to T included The final configuration of the trajectory is thus the one indexed T and there an extra one for convinience in trajectory optimization algorithm (for finite differences). Hence when we define a spline we need to specify T configurations over a time interval that finishes at dt * (T+1).

acceleration(t)

returns acceleration at index i Note that we suppose velocity at q_init to be 0

config_at_time(t)
initialize_spline()
time_indices()
velocity(t)

returns velocity at index i WARNING It is not the same convention as for the clique

Here we suppose the velocity at q_init to be 0, so the finite difference is left sided (q_t - q_t-1)/dt This is different from the right sided version (q_t+1 - q_t)/dt implemented in the cost term module.

With left side FD we directly get the integration scheme:

q_{t+1} = q_t + v_t * dt + a_t * dt^2

where v_t and a_t are velocity and acceleration at index t, with v_0 = 0.

class pyrieef.motion.trajectory.FunctionNetwork

Bases: geometry.differentiable_geometry.DifferentiableMap

Base class to implement a function network It registers functions and evaluates f(x_0, x_1, … ) = sum_i f_i(x_0, x_1, …)

add_function(f)
forward(x)

Should return an array or single value

input_dimension()
output_dimension()
class pyrieef.motion.trajectory.Trajectory(T=0, n=2, q_init=None, x=None)

Bases: object

Implement a trajectory as a single vector of configuration, returns cliques of configurations Note there is T active configuration in the trajectory indices

0 and T + 1

are supposed to be inactive.

T()
acceleration(i, dt)
returns acceleration at index i

Note that we suppose velocity at q_init to be 0

active_segment()

The active segment of the trajectory removes the first configuration on the trajectory

clique(i)

returns a clique of 3 configurations

configuration(i)

mutable : traj.configuration(3)[:] = np.ones(2)

continuous_trajectory()

returns an object of contunious type

final_configuration()

last active configuration

initial_configuration()

first configuration

list_configurations()

returns a list of configurations

n()
set(x)
set_from_configurations(configurations)

Takes a list of configuration and set them as a trajectory (equaly time spaced config)

state(i, dt)

return a tuple of configuration and velocity at index i

velocity(i, dt)
returns velocity at index i
WARNING It is not the same convention as for the clique

Here we suppose the velocity at q_init to be 0, so the finite difference is left sided (q_t - q_t-1)/dt This is different from the right sided version (q_t+1 - q_t)/dt implemented in the cost term module.

With left side FD we directly get the integration scheme:

q_{t+1} = q_t + v_t * dt + a_t * dt^2

where v_t and a_t are velocity and acceleration at index t, with v_0 = 0.

x()
class pyrieef.motion.trajectory.TrajectoryObjectiveFunction(q_init, function_network)

Bases: geometry.differentiable_geometry.DifferentiableMap

Wraps the active part of the Clique Function Network

The idea is that the finite differences are approximated using cliques so for the first clique, these values are quite off. Since the first configuration is part of the problem statement we can take it out of the optimization and through away the gradient computed for that configuration.

TODO Test…

forward(x)

Should return an array or single value

full_vector(x_active)
hessian(x)
Should return the hessian matrix

n x n : input x input (dimensions)

by default the method returns the finite difference hessian that relies on the jacobian function. This method would be a third order tensor in the case of multiple output, we exclude this case for now. WARNING the object returned by this function is a numpy matrix.

input_dimension()
jacobian(x)
Should return a matrix or single value of

m x n : ouput x input (dimensions)

by default the method returns the finite difference jacobian. WARNING the object returned by this function is a numpy matrix.

output_dimension()
pyrieef.motion.trajectory.linear_interpolation_trajectory(q_init, q_goal, T)
pyrieef.motion.trajectory.no_motion_trajectory(q_init, T)
pyrieef.motion.trajectory.resample(trajectory, T)

interpolate

Module contents