horizon.utils package

Submodules

horizon.utils.collision module

class horizon.utils.collision.CollisionHandler(urdfstr, kindyn)

Bases: object

classmethod clamp(x, xmin, xmax)

classmethod collision_to_capsule(link: urdf_parser_py.urdf.Link)

[summary]

Parameters:link (urdf.Link) – [description] Returns:[description] Return type:[type]

compute_distances(q)

classmethod dist_capsule_capsule(capsule_1: urdf_parser_py.urdf.Collision, T1, capsule_2: urdf_parser_py.urdf.Collision, T2)

classmethod dist_segment_segment(p_FP1, p_FQ1, p_FP2, p_FQ2)

get_function()

horizon.utils.kin_dyn module

class horizon.utils.kin_dyn.ForwardDynamics(kindyn, contact_frames=[], force_reference_frame=<ReferenceFrame.LOCAL: 1>)

Bases: object

Class which computes forward dynamics: given generalized position, velocities, torques and contact forces, returns generalized accelerations

call(q, qdot, tau, frame_force_mapping={})

Computes generalized accelerations: :param q: joint positions :param qdot: joint velocities :param torques: joint torques :param frame_force_mapping: dictionary containing a map between frames and force variables e.g. {‘lsole’: F1} representing the frame

where the force is acting (the force is expressed in force_reference_frame!)

Returns:generalized accelerations Return type:qddot

class horizon.utils.kin_dyn.InverseDynamics(kindyn, contact_frames=[], force_reference_frame=<ReferenceFrame.LOCAL: 1>)

Bases: object

Class which computes inverse dynamics: given generalized position, velocities, accelerations and contact forces, returns generalized torques

call(q, qdot, qddot, frame_force_mapping={}, tau_ext=0)

Computes generalized torques: :param q: joint positions :param qdot: joint velocities :param qddot: joint accelerations :param frame_force_mapping: dictionary containing a map between frames and force variables e.g. {‘lsole’: F1} representing the frame

where the force is acting (the force is expressed in force_reference_frame!)

Parameters:tau_ext – extra torques input for dynamics Returns:generalized torques Return type:tau

class horizon.utils.kin_dyn.InverseDynamicsMap(N, kindyn, contact_frames=[], force_reference_frame=<ReferenceFrame.LOCAL: 1>)

Bases: object

Class which computes inverse dynamics: given generalized position, velocities, accelerations and contact forces, returns generalized torques

call(q, qdot, qddot, frame_force_mapping={})

Computes generalized torques: :param q: joint positions :param qdot: joint velocities :param qddot: joint accelerations :param frame_force_mapping: dictionary containing a map between frames and force variables e.g. {‘lsole’: F1} representing the frame

where the force is acting (the force is expressed in force_reference_frame!)

Returns:generalized torques Return type:tau

horizon.utils.kin_dyn.linearized_friction_cone(f, mu, R)

Parameters:

• f – force (only linear components)
• mu – friciton cone
• R – rotation matrix between force reference frame and surface frame (ff_R_cf, ff: force frame, cf: contact frame)

Returns:

constraint lb ub

horizon.utils.kin_dyn.linearized_friction_cone_map(f, mu, R, N)

Parameters:

• f – force (only linear components)
• mu – friciton cone
• R – rotation matrix between force reference frame and surface frame (ff_R_cf, ff: force frame, cf: contact frame)

Returns:

constraint lb ub

horizon.utils.kin_dyn.surface_point_contact(plane_dict, q, kindyn, frame)

Position ONLY constraint to lies into a plane: ax + by + cz +d = 0 todo:Add orientation as well :param plane_dict:

which contains following variables:

• a
• b
• c
• d

to define the plane ax + by + cz +d = 0

Parameters:

• q – position state variables
• kindyn – casadi_kin_dyn object
• frame – name of the point frame constrained in the plane

Returns:

constraint lb ub

horizon.utils.mat_storer module

class horizon.utils.mat_storer.matStorer(file_name)

Bases: object

append(dict_values)

load()

save()

store(dict_values)

class horizon.utils.mat_storer.matStorerIO

Bases: object

append(dict_values)

argParse()

load(solution: dict)

save()

store(dict_values)

horizon.utils.mat_storer.setInitialGuess(variables: dict, solution: dict)

horizon.utils.plotter module

class horizon.utils.plotter.PlotterHorizon(prb: horizon.problem.Problem, solution=None, opts=None, logger=None)

Bases: object

plotFunction(name, grid=False, markers=None, show_bounds=None, legend=None, dim=None)

plotFunctions(grid=False, gather=None, markers=None, show_bounds=None, legend=None, dim=None)

plotVariable(name, grid=False, markers=None, show_bounds=None, legend=None, dim=None)

plotVariables(names=None, grid=False, gather=None, markers=False, show_bounds=True, legend=True, dim=None)

setSolution(solution)

horizon.utils.refiner module

class horizon.utils.refiner.Refiner(prb: horizon.problem.Problem, new_nodes_vec, solver)

Bases: object

addProximalCosts()

expandDt()

expand_nodes(vec_to_expand)

find_nodes_to_inject(vec_to_expand)

getAugmentedProblem()

getSolution()

get_node_time(dt)

group_elements(vec)

resetFunctions()

resetInitialGuess()

resetProblem()

resetVarBounds()

solveProblem()

horizon.utils.replayer module

horizon.utils.replayer_fd module

horizon.utils.replayer_fd_mx module

horizon.utils.replayer_mx module

horizon.utils.resampler_trajectory module

horizon.utils.resampler_trajectory.resample_input(input, node_time, dt)

Resample an input variable according to a new sample time dt. NOTE: the resampling is done by considering constant input between nodes :param input: input to resample :param node_time: original node time :param dt: new node time

Returns:resampled input Return type:input_res

horizon.utils.resampler_trajectory.resample_torques(p, v, a, node_time, dt, dae, frame_force_mapping, kindyn, force_reference_frame=<ReferenceFrame.LOCAL: 1>)

Resample solution to a different number of nodes, RK4 integrator is used for the resampling :param p: position :param v: velocity :param a: acceleration :param node_time: previous node time :param dt: resampled period :param dae: a dictionary containing

‘x’: state ‘p’: control ‘ode’: a function of the state and control returning the derivative of the state ‘quad’: quadrature term

Parameters:

• frame_force_mapping – dictionary containing a map between frames and force variables e.g. {‘lsole’: F1}
• kindyn – object of type casadi_kin_dyn
• force_reference_frame – this is the frame which is used to compute the Jacobian during the ID computation: LOCAL (default) WORLD LOCAL_WORLD_ALIGNED

Returns:

resampled p v_res: resampled v a_res: resampled a frame_res_force_mapping: resampled frame_force_mapping tau_res: resampled tau

Return type:

p_res

horizon.utils.resampler_trajectory.resampler(state_vec, input_vec, nodes_dt, desired_dt, dae)

horizon.utils.resampler_trajectory.second_order_resample_integrator(p, v, u, node_time, dt, dae)

Resample a solution with the given dt (RK4 integrator is used internally) :param p: position :param v: velocity :param u: input :param node_time: previous node time :param dt: resampling time :param dae: dynamic model

Returns:resampled position v_res: resampled velocity u_res: resampled input Return type:p_res

horizon.utils.rti module

class horizon.utils.rti.RealTimeIteration(prb: horizon.problem.Problem, solver: horizon.solvers.solver.Solver, dt: float)

Bases: object

integrate(stateread: numpy.array, u_opt: numpy.array)

run(stateread: numpy.array)

horizon.utils.send_to_gazebo module

horizon.utils.utils module

horizon.utils.utils.double_integrator(q, qdot, qddot)

horizon.utils.utils.double_integrator_with_floating_base(q, ndot, nddot)

Construct the floating-base dynamic model:

x = [q, ndot] xdot = [qdot, nddot]

using quaternion dynamics: quatdot = quat x [omega, 0] NOTE: this implementation consider floating-base position and orientation expressed in GLOBAL (world) coordinates while linear and angular velocities expressed in LOCAL (base_link) coordinates. :param q: joint space coordinates: q = [x y z px py pz pw qj], where p is a quaternion :param ndot: joint space velocities: ndot = [vx vy vz wx wy wz qdotj] :param nddot: joint space acceleration: nddot = [ax ay ax wdotx wdoty wdotz qddotj]

Returns:state x = [q, ndot] xdot: derivative of the state xdot = [qdot, nddot] Return type:x

horizon.utils.utils.jac(dict, var_string_list, function_string_list)

Parameters:

• dict – dictionary which maps variables and functions, eg. {‘x’: x, ‘u’: u, ‘f’: f}
• var_string_list – list of variables in dict, eg. [‘x’, ‘u’]
• function_string_list – list of functions in dict, eg. [‘f’]

Returns:

casadi Function for evaluation jac: dictionary with expression of derivatives

Return type:

F

NOTE: check /tests/jac_test.py for example of usage for Jacobian and Hessian computation

horizon.utils.utils.quaterion_product(q, p)

Computes quaternion product between two quaternions q and p :param q: quaternion :param p: quaternion

Returns:quaternion product q x p

horizon.utils.utils.skew(q)

Create skew matrix from vector part of quaternion :param q: vector part of quaternion [qx, qy, qz]

Returns:S = skew symmetric matrix built using q

horizon.utils.utils.toRot(q)

Compute rotation matrix associated to given quaternion q :param q: quaternion

Returns:rotation matrix Return type:R

horizon.utils.vis_refiner_global module

Module contents