#include <math.h>
#include <float.h>
#include <stdlib.h>
#include <memory>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <limits>
#include <cmath>
#include "snopt.hh"
#include "snfilewrapper.hh"
#include "drake/systems/plants/RigidBodyIK.h"
#include "drake/systems/plants/RigidBodyTree.h"
#include "constraint/RigidBodyConstraint.h"
#include "drake/systems/plants/IKoptions.h"
#include "inverseKinBackend.h"
#include <Eigen/LU>

Include dependency graph for inverseKinSnoptBackend.cpp:

Namespaces
	snopt

	Drake
	NOTE: The contents of this class are for the most part direct ports of drake/systems/plants//inverseKinBackend.m from Matlab to C++; many methods and variables follow Matlab conventions and are documented in that file.

	Drake::systems

	Drake::systems::plants

Functions
static void	gevalNumerical (void(*func_ptr)(const VectorXd &, VectorXd &), const VectorXd &x, VectorXd &c, MatrixXd &dc, int order=2)

static void	IK_constraint_fun (KinematicsCache< double > &cache, double x, double c, double *G)

static void	IK_cost_fun (double x, double &J, double dJ)

static int	snoptIKfun (snopt::integer Status, snopt::integer n, snopt::doublereal x[], snopt::integer needF, snopt::integer neF, snopt::doublereal F[], snopt::integer needG, snopt::integer neG, snopt::doublereal G[], char cu, snopt::integer lencu, snopt::integer iu[], snopt::integer leniu, snopt::doublereal ru[], snopt::integer lenru)

static void	IKtraj_cost_fun (MatrixXd q, const VectorXd &qdot0, const VectorXd &qdotf, double &J, double *dJ)

static int	snoptIKtrajfun (snopt::integer Status, snopt::integer n, snopt::doublereal x[], snopt::integer needF, snopt::integer neF, snopt::doublereal F[], snopt::integer needG, snopt::integer neG, snopt::doublereal G[], char cu, snopt::integer lencu, snopt::integer iu[], snopt::integer leniu, snopt::doublereal ru[], snopt::integer lenru)

static void	snoptIKtraj_userfun (const VectorXd &x_vec, VectorXd &c_vec, VectorXd &G_vec)

static void	snoptIKtraj_fevalfun (const VectorXd &x, VectorXd &c)

template<typename DerivedA , typename DerivedB , typename DerivedC , typename DerivedD , typename DerivedE >
void	inverseKinSnoptBackend (RigidBodyTree model_input, const int mode, const int nT_input, const double t_input, const MatrixBase< DerivedA > &q_seed, const MatrixBase< DerivedB > &q_nom_input, const int num_constraints, RigidBodyConstraint *const constraint_array, const IKoptions &ikoptions, MatrixBase< DerivedC > q_sol, MatrixBase< DerivedD > qdot_sol, MatrixBase< DerivedE > qddot_sol, int INFO, std::vector< std::string > infeasible_constraint)

template void	inverseKinSnoptBackend (RigidBodyTree model, const int mode, const int nT, const double t, const MatrixBase< Map< MatrixXd >> &q_seed, const MatrixBase< Map< MatrixXd >> &q_nom, const int num_constraints, RigidBodyConstraint *const constraint_array, const IKoptions &ikoptions, MatrixBase< Map< MatrixXd >> q_sol, MatrixBase< Map< MatrixXd >> qdot_sol, MatrixBase< Map< MatrixXd >> qddot_sol, int INFO, std::vector< std::string > infeasible_constraint)

template void	inverseKinSnoptBackend (RigidBodyTree model, const int mode, const int nT, const double t, const MatrixBase< MatrixXd > &q_seed, const MatrixBase< MatrixXd > &q_nom, const int num_constraints, RigidBodyConstraint *const constraint_array, const IKoptions &ikoptions, MatrixBase< MatrixXd > q_sol, MatrixBase< MatrixXd > qdot_sol, MatrixBase< MatrixXd > qddot_sol, int INFO, std::vector< std::string > infeasible_constraint)

template void	inverseKinSnoptBackend (RigidBodyTree model, const int mode, const int nT, const double t, const MatrixBase< Map< MatrixXd >> &q_seed, const MatrixBase< Map< MatrixXd >> &q_nom, const int num_constraints, RigidBodyConstraint *const constraint_array, const IKoptions &ikoptions, MatrixBase< Map< MatrixXd >> q_sol, MatrixBase< MatrixXd > qdot_sol, MatrixBase< MatrixXd > qddot_sol, int INFO, std::vector< std::string > infeasible_constraint)

template void	inverseKinSnoptBackend (RigidBodyTree model, const int mode, const int nT, const double t, const MatrixBase< Map< VectorXd >> &q_seed, const MatrixBase< Map< VectorXd >> &q_nom, const int num_constraints, RigidBodyConstraint *const constraint_array, const IKoptions &ikoptions, MatrixBase< Map< VectorXd >> q_sol, MatrixBase< Map< VectorXd >> qdot_sol, MatrixBase< Map< VectorXd >> qddot_sol, int INFO, std::vector< std::string > infeasible_constraint)

template void	inverseKinSnoptBackend (RigidBodyTree model, const int mode, const int nT, const double t, const MatrixBase< VectorXd > &q_seed, const MatrixBase< VectorXd > &q_nom, const int num_constraints, RigidBodyConstraint *const constraint_array, const IKoptions &ikoptions, MatrixBase< VectorXd > q_sol, MatrixBase< VectorXd > qdot_sol, MatrixBase< VectorXd > qddot_sol, int INFO, std::vector< std::string > infeasible_constraint)

template void	inverseKinSnoptBackend (RigidBodyTree model, const int mode, const int nT, const double t, const MatrixBase< Map< VectorXd >> &q_seed, const MatrixBase< Map< VectorXd >> &q_nom, const int num_constraints, RigidBodyConstraint *const constraint_array, const IKoptions &ikoptions, MatrixBase< Map< VectorXd >> q_sol, MatrixBase< VectorXd > qdot_sol, MatrixBase< VectorXd > qddot_sol, int INFO, std::vector< std::string > infeasible_constraint)

Variables
static unique_ptr< snopt::doublereal[]>	rw

static unique_ptr< snopt::integer[]>	iw

static unique_ptr< char[]>	cw

static snopt::integer	lenrw = 0

static snopt::integer	leniw = 0

static snopt::integer	lencw = 0

static RigidBodyTree *	model = nullptr

static SingleTimeKinematicConstraint **	st_kc_array = nullptr

static MultipleTimeKinematicConstraint **	mt_kc_array = nullptr

static QuasiStaticConstraint *	qsc_ptr = nullptr

static MatrixXd	q_nom

static VectorXd	q_nom_i

static MatrixXd	Q

static MatrixXd	Qa

static MatrixXd	Qv

static bool	qscActiveFlag

static snopt::integer	nx

static snopt::integer	nF

static snopt::integer	nG

static snopt::integer *	nc_array

static snopt::integer *	nG_array

static snopt::integer *	nA_array

static int	nq

static int	nv

static double *	t = nullptr

static double *	t_samples = nullptr

static double *	ti = nullptr

static int	num_st_kc

static int	num_mt_kc

static int	num_st_lpc

static int	num_mt_lpc

static int *	mt_kc_nc

static int *	st_lpc_nc

static int *	mt_lpc_nc

static int	nT

static int	num_qsc_pts

static bool	fixInitialState

static snopt::integer *	qfree_idx

static snopt::integer *	qdotf_idx

static snopt::integer *	qdot0_idx

static VectorXd	q0_fixed

static VectorXd	qdot0_fixed

static snopt::integer	qstart_idx

static snopt::integer	num_qfree

static snopt::integer	num_qdotfree

static MatrixXd	velocity_mat

static MatrixXd	velocity_mat_qd0

static MatrixXd	velocity_mat_qdf

static MatrixXd	accel_mat

static MatrixXd	accel_mat_qd0

static MatrixXd	accel_mat_qdf

static VectorXd *	t_inbetween

static snopt::integer	num_inbetween_tSamples

static MatrixXd *	dqInbetweendqknot

static MatrixXd *	dqInbetweendqd0

static MatrixXd *	dqInbetweendqdf

static snopt::integer *	qknot_qsamples_idx

static snopt::integer	nF_tmp

static snopt::integer	nG_tmp

static snopt::integer	nx_tmp

Namespaces

Functions

Variables