Drake
 All Classes Files Functions Variables
ComDynamicsFullKinematicsPlanner Class Reference

This planner impose the following dynamics constraint kc_com(q) = com at evey t_knot H(:,i) -H(:,i-1) = Hdot(:,i)*dt(i) Hdot(:,i) = sum_j cross(p_contact_j-com(:,i),F_j) com(:,i) - com(:,i-1) = comdot(:,i)*dt(i) comdot(:,i)-comdot(:,i-1) = comddot(:,i)*dt(i) m*comddot(:,i) = sum_j F_j-m*g q(:,i)-q(:,i-1) = v(:,i)*dt(i) A*v(:,i) = H(:,i) where A = robot.centroidalMomentumMatrix. More...

Inheritance diagram for ComDynamicsFullKinematicsPlanner:
Collaboration diagram for ComDynamicsFullKinematicsPlanner:

Public Member Functions

function ComDynamicsFullKinematicsPlanner (robot, N, tf_range, Q_comddot, Qv, Q, q_nom, Q_contact_force, contact_wrench_struct, options)
 
function addConstraint (obj, constraint, varargin)
 
function addCoMBounds (obj, knot_idx, com_lb, com_ub)
 
function addRunningCost (obj, running_cost)
 
function parseSolution (obj, x_sol)
 
function addDynamicConstraints (obj)
 
- Public Member Functions inherited from SimpleDynamicsFullKinematicsPlanner
function SimpleDynamicsFullKinematicsPlanner (plant, robot, N, tf_range, Q_contact_force, contact_wrench_struct, options)
 
function kinematicsData (obj, q, kinematics_cache_ptr)
 
function setFixInitialState (obj, flag, x0)
 set obj.fix_initial_state = flag. More...
 
function addKinematicConstraint (obj, constraint, time_index)
 Add a kinematic constraint that is a function of the state at the specified time or times. More...
 
function addRigidBodyConstraint (obj, constraint, time_index)
 Add a kinematic constraint that is a function of the state at the specified time or times. More...
 
function contactWrench (obj, x)
 Given x as the decision variables, find out the contact wrench. More...
 
- Public Member Functions inherited from DirectTrajectoryOptimization
function DirectTrajectoryOptimization (plant, N, durations, options)
 function obj = DirectTrajectoryOptimization(plant,initial_cost,running_cost,final_cost, % t_init,traj_init,T_span,constraints, options) Trajectory optimization constructor More...
 
function getN (obj)
 
function getXinds (obj)
 
function getHinds (obj)
 
function addInputConstraint (obj, constraint, time_index)
 Add constraint (or composite constraint) that is a function of the input at the specified time or times. More...
 
function addStateConstraint (obj, constraint, time_index, x_indices)
 Add constraint (or composite constraint) that is a function of the state at the specified time or times. More...
 
function addTrajectoryDisplayFunction (obj, display_fun)
 add a dispay function that gets called on every iteration of the algorithm More...
 
function solveTraj (obj, t_init, traj_init)
 Solve the nonlinear program and return resulting trajectory. More...
 
function getInitialVars (obj, t_init, traj_init)
 evaluates the initial trajectories at the sampled times and constructs the nominal z0. More...
 
function setupVariables (obj, N)
 Default implementation, Assumes, if time is not fixed, that there are N-1 time steps N corresponding state variables and N-1 corresponding input variables Overwrite to change. More...
 
function addInitialCost (obj, initial_cost)
 Adds a cost to the initial state f(x0) More...
 
function addFinalCost (obj, final_cost_function)
 adds a cost to the final state and total time More...
 
function reconstructInputTrajectory (obj, z)
 default behavior is to use first order holds, but this can be re-implemented by a subclass. More...
 
function reconstructStateTrajectory (obj, z)
 default behavior is to use first order holds, but this can be re-implemented by a subclass. More...
 
function extractFirstInput (obj, z)
 When using trajectory optimization a part of a model-predictive control system, we often only need to extract u(0). More...
 
virtual function addRunningCost (obj, running_cost_function)
 Adds an integrated cost to all time steps, which is numerical implementation specific (thus abstract) this cost is assumed to be time-invariant. More...
 
- Public Member Functions inherited from NonlinearProgram
function NonlinearProgram (num_vars, x_name)
 
function addCompositeConstraints (obj, cnstr, xind, data_ind)
 add a CompositeConstraint to the object, change the constraint evalation of the program. More...
 
function addConstraint (obj, cnstr, varargin)
 obj = addConstraint(obj,cnstr,varargin) Queries the constraint type and calls the appropriate addConstraint method (e.g. More...
 
function addNonlinearConstraint (obj, cnstr, xind, data_ind)
 add a NonlinearConstraint to the object, change the constraint evalation of the program. More...
 
function addLinearConstraint (obj, cnstr, xind)
 add a LinearConstraint to the program More...
 
function addBoundingBoxConstraint (obj, cnstr, xind)
 add a BoundingBoxConstraint to the program More...
 
function addCost (obj, cnstr, xind, data_ind)
 Add a cost to the objective function. More...
 
function addQuadraticCost (obj, Q, x_desired, xind)
 helper function for the very common case of adding the objective g(x) = (x-xd)'Q(x-xd), Q = Q' >= 0 More...
 
function getArgumentArray (obj, x, xind)
 Retrieves the elements from the vector x related to xind and returns them as a cell array where: args{i} = x(xind{i}) More...
 
function nonlinearConstraints (obj, x)
 evaluate the nonlinear constraints More...
 
function objective (obj, x)
 return the value of the objective More...
 
function objectiveAndNonlinearConstraints (obj, x)
 evaluate the objective and the nonlinear constraints altogher More...
 
function addDecisionVariable (obj, num_new_vars, var_name)
 appending new decision variables to the end of the current decision variables More...
 
function replaceCost (obj, cost, cost_idx, xind)
 replace the cost_idx'th cost in the original problem with a new cost More...
 
function addSharedDataFunction (obj, user_fun, xind)
 Adds the specified shared data function to be evaluated within each iteration of the program. More...
 
function getNumSharedDataFunctions (obj)
 
function evaluateSharedDataFunctions (obj, x)
 Evaluate all shared data functions and return the data object. More...
 
function addDisplayFunction (obj, display_fun, indices)
 add a dispay function that gets called on every iteration of the algorithm More...
 
function setCheckGrad (obj, check_grad)
 
function setConstraintErrTol (obj, tol)
 
function setSolver (obj, solver)
 
function setSolverOptions (obj, solver, optionname, optionval)
 
function getNonlinearGradientSparsity (obj)
 This function sets the nonlinear sparsity vector iGfun and jGvar based on the nonlinear sparsity of the objective, nonlinear inequality constraints and nonlinear equality constraints. More...
 
function bounds (obj)
 return the bounds for all the objective function, nonlinear constraints and linear constraints More...
 
function solve (obj, x0)
 
function compareSolvers (obj, x0, solvers)
 
function isNonlinearConstraintID (obj, cnstr_id)
 Given an ID, determine if any of the nonlinear constraint obj.nlcon has that ID. More...
 
function isLinearConstraintID (obj, cnstr_id)
 Given an ID, determine if any of the linear constraint obj.lcon has that ID. More...
 
function isBoundingBoxConstraintID (obj, cnstr_id)
 Given an ID, determine if any of the bounding box constraint obj.bbcon has that ID. More...
 
function deleteConstraint (obj, delete_cnstr_id)
 delete a constraint from the program More...
 
function updateConstraint (obj, varargin)
 update a Constraint of the program. More...
 
function deleteNonlinearConstraint (obj, delete_cnstr_id)
 delete a nonlinear constraint from the program More...
 
function updateNonlinearConstraint (obj, varargin)
 updateNonlinearConstraint(obj,cnstr_id,cnstr,xind,data_ind) update the nonlinear constraint whose id=cnstr_id with a new Constraint object cnstr, the newly added Constraint cnstr has the ID new_cnstr_id More...
 
function deleteLinearConstraint (obj, delete_cnstr_id)
 delete the LinearConstraint obj.lcon{cnstr_idx} from the program More...
 
function updateLinearConstraint (obj, varargin)
 updateLinearConstraint(obj,cnstr_id,cnstr,xind) update the linear constraint whose id=cnstr_id with a new Constraint object cnstr, the newly added Constraint cnstr has the ID new_cnstr_id More...
 
function deleteBoundingBoxConstraint (obj, cnstr_id)
 delete the BoundingBoxConstraint in obj.bbcon with ID=cnstr_id from the program More...
 
function updateBoundingBoxConstraint (obj, varargin)
 updateBoundingBoxConstraint(obj,cnstr_id,cnstr,xind) update the BoundingBoxConstraint whose id=cnstr_id with a new BoundingBoxConstraint cnstr More...
 

Protected Member Functions

function addSimpleDynamicConstraints (obj)
 
function addCentroidalDynamicConstraints (obj)
 
function addContactDynamicConstraints (obj, knot_idx, contact_wrench_idx, knot_lambda_idx)
 
- Protected Member Functions inherited from SimpleDynamicsFullKinematicsPlanner
function parseRigidBodyContactWrench (obj)
 
function addForceNormCost (obj)
 add a quadratic cost on sum_i,j force_j[i]'*obj.Q_contact_force*force_j[i] More...
 
function addSimpleDynamicConstraints (obj)
 First I find out the order of the contact_wrench such that it is in the same order of lambda. More...
 
virtual function addContactDynamicConstraints (obj, num_knot, contact_wrench_idx, knot_lambda_idx)
 
- Protected Member Functions inherited from DirectTrajectoryOptimization
function final_cost (obj, final_cost_function, h, x)
 
- Protected Member Functions inherited from NonlinearProgram
function snopt (obj, x0)
 
function fmincon (obj, x0)
 if (obj.num_cin + obj.num_ceq) nonlinearConstraints = .nonlinearConstraint; else nonlinearConstraints = []; end More...
 
function ipopt (obj, x0)
 
function setVarBounds (obj, lb, ub)
 
function mapSolverInfo (obj, exitflag, x)
 Based on the solver information and solution, re-map the info. More...
 

Protected Attributes

Property com_inds
 
Property comdot_inds
 
Property comddot_inds
 
Property H_inds
 
Property Hdot_inds
 
Property torque_multiplier
 
Property ncp_tol
 
- Protected Attributes inherited from SimpleDynamicsFullKinematicsPlanner
Property robot
 
Property nq
 
Property nv
 
Property q_inds
 
Property v_inds
 
Property qsc_weight_inds
 
Property fix_initial_state
 
Property g
 
Property kinsol_dataind
 N-element vector of indices into the shared_data, where shared_data{kinsol_dataind(i)} is the kinsol for knot point i. More...
 
Property kinematics_cache_ptrs
 kinematics cache pointers, one for each knot point More...
 
Property contact_wrench
 
Property contact_wrench_active_knot
 
Property unique_contact_bodies
 
Property unique_body_contact_pts
 
Property lambda_inds
 
Property lambda2contact_wrench
 
Property num_lambda_knot
 lambda{i}(:,j,k) are the contact forces whose information are encoded in the RigidBodyContactWrench obj.contact_wrench(obj.lambda2contact_wrench{i}(j,k)) If there is no active RigidBodyContactWrench, then the value of lambda2contact_wrench is set to be 0. More...
 
Property robot_mass
 
Property Q_contact_force
 
- Protected Attributes inherited from DirectTrajectoryOptimization
Property N
 
Property options
 
Property plant
 
Property h_inds
 
Property x_inds
 
Property u_inds
 
Property dynamic_constraints
 
Property constraints
 
- Protected Attributes inherited from NonlinearProgram
Property num_vars
 
Property num_cin
 
Property num_ceq
 
Property Ain
 
Property bin
 
Property Ain_name
 
Property Aeq
 
Property beq
 
Property Aeq_name
 
Property cin_lb
 
Property cin_ub
 
Property cin_name
 
Property ceq_name
 
Property x_lb
 
Property x_ub
 
Property x_name
 
Property solver
 
Property solver_options
 
Property display_funs
 
Property display_fun_indices
 
Property check_grad
 
Property constraint_err_tol
 numerical gradient at the begining and end of the nonlinear optimization More...
 
Property nlcon
 
Property lcon
 
Property bbcon
 
Property cost
 
Property nlcon_xind
 
Property bbcon_xind
 nlcon{i}.eval(x(nlcon_xind{i}{1},x(nlcon_xind{i}{2},) cost_xind_cell % A cell array, cost_xind{i} is a cell array of int vectors recording the indices of x that is used in evaluating obj.cost{i} More...
 
Property shared_data_xind_cell
 a cell array like nlcon_xind, where shared_data_xind_cell{i} is a cell array of int vectors recording indices used in evaluating the shared_data_function More...
 
Property nlcon_dataind
 a cell array of function handles, each of which returns a data object so that shared_data{i} = shared_data_functions(x(shared_data_xind_cell{i}{1}),x(shared_data_xind_cell{i}{2}),) shared_data_functions More...
 
Property cost_dataind
 
Property iFfun
 2 if user has their own snopt in MATLAB path More...
 
Property iCinfun
 
Property iCeqfun
 

Detailed Description

This planner impose the following dynamics constraint kc_com(q) = com at evey t_knot H(:,i) -H(:,i-1) = Hdot(:,i)*dt(i) Hdot(:,i) = sum_j cross(p_contact_j-com(:,i),F_j) com(:,i) - com(:,i-1) = comdot(:,i)*dt(i) comdot(:,i)-comdot(:,i-1) = comddot(:,i)*dt(i) m*comddot(:,i) = sum_j F_j-m*g q(:,i)-q(:,i-1) = v(:,i)*dt(i) A*v(:,i) = H(:,i) where A = robot.centroidalMomentumMatrix.

Constructor & Destructor Documentation

function ComDynamicsFullKinematicsPlanner ( robot  ,
N  ,
tf_range  ,
Q_comddot  ,
Qv  ,
,
q_nom  ,
Q_contact_force  ,
contact_wrench_struct  ,
options   
)
Parameters
Q_comddotA 3 x 3 matrix. penalize sum_j comddot(:,j)*Q_comddot*comddot(:,j)
Qan nq x nq matrix. Add the cost sum_j (q(:,j)-q_nom(:,j))'Q(q(:,j)-q_nom(:,j));
Return values
obj

Member Function Documentation

function addCentroidalDynamicConstraints ( obj  )
protected
Return values
obj
function addCoMBounds ( obj  ,
knot_idx  ,
com_lb  ,
com_ub   
)
Parameters
knot_idxThe indices of the knots on which the com position will be constrained within the bounding box.
Return values
obj
function addConstraint ( obj  ,
constraint  ,
varargin   
)
Return values
obj
function addContactDynamicConstraints ( obj  ,
knot_idx  ,
contact_wrench_idx  ,
knot_lambda_idx   
)
protected
Return values
obj
function addDynamicConstraints ( obj  )
virtual
Return values
obj

Reimplemented from DirectTrajectoryOptimization.

function addRunningCost ( obj  ,
running_cost   
)
Return values
obj
function addSimpleDynamicConstraints ( obj  )
protected
Return values
obj
function parseSolution ( obj  ,
x_sol   
)
Return values
q
v
h
t
com
comdot
comddot
H
Hdot
lambda
wrench

Member Data Documentation

Property com_inds
protected
Property comddot_inds
protected
Property comdot_inds
protected
Property H_inds
protected
Property Hdot_inds
protected
Property ncp_tol
protected
Property torque_multiplier
protected

The documentation for this class was generated from the following file: