Drake
InverseKinematicsTrajectory Class Reference

solve IK min_q sum_i qdd(:,i)'Qa*qdd(:,i)+qd(:,i)'*Qv*qd(:,i)+(q(:,i)-q_nom(:,i))'*Q(q(:,i)-q_nom(:,i))]+additional_cost1+additional_cost2+ % subject to constraint1 at t_samples(i) constraint2 at t_samples(i) % constraint(k) at [t_samples(1) t_samples(2) t_samples(nT)] % constraint(k+1) at [t_samples(1) t_samples(2) t_samples(nT)] % . More...

Inheritance diagram for InverseKinematicsTrajectory:
Collaboration diagram for InverseKinematicsTrajectory:

## Public Member Functions

function InverseKinematicsTrajectory (robot, t, q_nom_traj, fix_initial_state, x0, varargin)
obj = InverseKinematicsTrajectory(robot,t,q_nom_traj,RigidBodyConstraint1,RigidBodyConstraint2,,RigidBodyConstraintN) % More...

function kinematicsData (obj, q)

function setCubicPostureError (obj, Q, Qv, Qa)
set the cost sum_i qdd(:,i)'Qa*qdd(:,i)+qd(:,i)'*Qv*qd(:,i)+(q(:,i)-q_nom(:,i))'*Q(q(:,i)-q_nom(:,i))] More...

function setFixInitialState (obj, flag, x0)
set obj.fix_initial_state = flag. More...

function solve (obj, qtraj_seed)

Add a constraint that is a function of the generalized positions at the specified time or times. 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...

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...

add a LinearConstraint to the program More...

add a BoundingBoxConstraint to the program More...

function addCost (obj, cnstr, xind, data_ind)
Add a cost to the objective function. More...

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...

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...

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...

add a dispay function that gets called on every iteration of the algorithm More...

function setConstraintErrTol (obj, tol)

function setSolver (obj, solver)

function setSolverOptions (obj, solver, optionname, optionval)

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 Attributes

Property t_knot

Property Q

Property q_nom_traj

Property Qv

Property Qa

Property rgc

Property fix_initial_state

Property q_idx

Property qd0_idx

Property qdf_idx

Property qsc_weight_idx

Property robot

Property kinsol_dataind
nT-element vector of indices into the shared_data, where shared_data{kinsol_dataind(i)} is the kinsol for knot point i More...

Property rbm_joint_bnd_cnstr_id

Property q_nom

Property cpe

Property nT

Property nq

Property t_kinsol

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 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

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...

## Detailed Description

solve IK min_q sum_i qdd(:,i)'Qa*qdd(:,i)+qd(:,i)'*Qv*qd(:,i)+(q(:,i)-q_nom(:,i))'*Q(q(:,i)-q_nom(:,i))]+additional_cost1+additional_cost2+ % subject to constraint1 at t_samples(i) constraint2 at t_samples(i) % constraint(k) at [t_samples(1) t_samples(2) t_samples(nT)] % constraint(k+1) at [t_samples(1) t_samples(2) t_samples(nT)] % .

% using q_seed_traj as the initial guess. q(1) would be fixed to q_seed_traj.eval(t(1))

Parameters
 robot – A RigidBodyManipulator or a TimeSteppingRigidBodyManipulator t_knot – A 1 x nT double vector. The t_knot(i) is the time of the i'th knot point Q – The matrix that penalizes the posture error q_nom_traj – The nominal posture trajectory Qv – The matrix that penalizes the velocity Qa – The matrix that penalizes the acceleration rgc – A cell of RigidBodyConstraint x_name – A string cell, x_name{i} is the name of the i'th decision variable fix_initial_state – A boolean flag. Set to true if the initial q(:,1) is hold constant at q_seed_traj.eval(t_knot(1)) and qdot(:,1) = q_seed_traj.fnder(1).eval(t_knot(1)). Otherwise the initial state is free to change. The default value is false. q_idx – a nq x nT matrix. q(:,i) = x(q_idx(:,i)) qd0_idx – a nq x 1 matrix. qdot0 = x(qd0_idx); qdf_idx – a nq x 1 matrix. qdotf = x(qdf_idx); qsc_weight_idx – a cell of vectors. x(qsc_weight_idx{i}) are the weights of the QuasiStaticConstraint at time t(i) rbm_joint_bnd_cnstr_id The ID of the BoundingBoxConstraint that enforces the posture to be within the joint limits of the RigidBodyManipulator

## Constructor & Destructor Documentation

 function InverseKinematicsTrajectory ( robot , t , q_nom_traj , fix_initial_state , x0 , varargin )
Parameters
 robot – A RigidBodyManipulator or a TimeSteppingRigidBodyManipulator t – A 1 x nT double vector. t(i) is the time of the i'th knot point q_nom_traj – The nominal posture trajectory fix_initial_state – A boolean flag, true if the [q(:,1);qdot(:,q)] is fixed to x0, and thus not a decision variable x0 – A 2*obj.nq x 1 vector. If fix_initial_state = true, then the initial state if fixed to x0, otherwise it is not used. RigidBodyConstraint_i – A RigidBodyConstraint object
Return values
 obj

## Member Function Documentation

 function addKinematicConstraint ( obj , constraint , time_index )

Add a constraint that is a function of the generalized positions at the specified time or times.

Parameters
 constraint a Constraint object time_index a cell array of time indices ex1., time_index = {1, 2, 3} means the constraint is applied individually to knot points 1, 2, and 3 ex2,. time_index = {[1 2], [3 4]} means the constraint is applied to knot points 1 and 2 together (taking the combined state as an argument) and 3 and 4 together.
Return values
 obj
 function kinematicsData ( obj , q )
Return values
 data
 function setCubicPostureError ( obj , Q , Qv , Qa )

set the cost sum_i qdd(:,i)'Qa*qdd(:,i)+qd(:,i)'*Qv*qd(:,i)+(q(:,i)-q_nom(:,i))'*Q(q(:,i)-q_nom(:,i))]

Return values
 obj
 function setFixInitialState ( obj , flag , x0 )

set obj.fix_initial_state = flag.

If flag = true, then fix the initial state to x0

Parameters
 x0 A 2*obj.nq x 1 double vector. x0 = [q0;qdot0]. The initial state
Return values
 obj
 function solve ( obj , qtraj_seed )
Parameters
 qtraj_seed. A Trajectory object. The initial guess of posture trajectory.
Return values
 xtraj. A Cubic spline trajectory. The solution of state trajectory, x = [q;qdot]

## Member Data Documentation

 Property cpe
protected
 Property fix_initial_state
protected
 Property kinsol_dataind
protected

nT-element vector of indices into the shared_data, where shared_data{kinsol_dataind(i)} is the kinsol for knot point i

 Property nq
protected
 Property nT
protected
 Property Q
protected
 Property q_idx
protected
 Property q_nom
protected
 Property q_nom_traj
protected
 Property Qa
protected
 Property qd0_idx
protected
 Property qdf_idx
protected
 Property qsc_weight_idx
protected
 Property Qv
protected
 Property rbm_joint_bnd_cnstr_id
protected
 Property rgc
protected
 Property robot
protected
 Property t_kinsol
protected
 Property t_knot
protected

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