#include "drake/systems/controllers/controlUtil.h"
#include "drake/math/autodiff.h"
#include "drake/math/expmap.h"
#include "drake/math/quaternion.h"
#include "drake/math/rotation_matrix.h"
#include "drake/util/drakeUtil.h"

Include dependency graph for controlUtil.cpp:

Functions
template<typename DerivedA , typename DerivedB >
void	getRows (std::set< int > &rows, MatrixBase< DerivedA > const &M, MatrixBase< DerivedB > &Msub)

template<typename DerivedA , typename DerivedB >
void	getCols (std::set< int > &cols, MatrixBase< DerivedA > const &M, MatrixBase< DerivedB > &Msub)

template<typename DerivedPhi1 , typename DerivedPhi2 , typename DerivedD >
void	angleDiff (MatrixBase< DerivedPhi1 > const &phi1, MatrixBase< DerivedPhi2 > const &phi2, MatrixBase< DerivedD > &d)

bool	inSupport (const std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &supports, int body_idx)

void	surfaceTangents (const Vector3d &normal, Matrix< double, 3, m_surface_tangents > &d)

int	contactPhi (const RigidBodyTree &r, const KinematicsCache< double > &cache, SupportStateElement &supp, VectorXd &phi)

int	contactConstraintsBV (const RigidBodyTree &r, const KinematicsCache< double > &cache, int nc, std::vector< double > support_mus, std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &supp, MatrixXd &B, MatrixXd &JB, MatrixXd &Jp, VectorXd &Jpdotv, MatrixXd &normals)

MatrixXd	individualSupportCOPs (const RigidBodyTree &r, const KinematicsCache< double > &cache, const std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &active_supports, const MatrixXd &normals, const MatrixXd &B, const VectorXd &beta)

bool	isSupportElementActive (SupportStateElement *se, bool contact_force_detected, bool kinematic_contact_detected)

Matrix< bool, Dynamic, 1 >	getActiveSupportMask (const RigidBodyTree &r, VectorXd q, VectorXd qd, std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &available_supports, const Ref< const Matrix< bool, Dynamic, 1 >> &contact_force_detected, double contact_threshold)

std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement > >	getActiveSupports (const RigidBodyTree &r, const VectorXd &q, const VectorXd &qd, std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &available_supports, const Ref< const Matrix< bool, Dynamic, 1 >> &contact_force_detected, double contact_threshold)

Vector6d	bodySpatialMotionPD (const RigidBodyTree &r, const DrakeRobotState &robot_state, const int body_index, const Isometry3d &body_pose_des, const Ref< const Vector6d > &body_v_des, const Ref< const Vector6d > &body_vdot_des, const Ref< const Vector6d > &Kp, const Ref< const Vector6d > &Kd, const Isometry3d &T_task_to_world)

void	evaluateXYZExpmapCubicSpline (double t, const PiecewisePolynomial< double > &spline, Isometry3d &body_pose_des, Vector6d &xyzdot_angular_vel, Vector6d &xyzddot_angular_accel)

void	getRobotJointIndexMap (JointNames joint_names, RobotJointIndexMap joint_map)

template DRAKECONTROLUTIL_EXPORT void	getRows (std::set< int > &, const MatrixBase< MatrixXd > &, MatrixBase< MatrixXd > &)

template DRAKECONTROLUTIL_EXPORT void	getCols (std::set< int > &, const MatrixBase< MatrixXd > &, MatrixBase< MatrixXd > &)

template DRAKECONTROLUTIL_EXPORT void	angleDiff (const MatrixBase< MatrixXd > &, const MatrixBase< MatrixXd > &, MatrixBase< MatrixXd > &)

template DRAKECONTROLUTIL_EXPORT void	angleDiff (const MatrixBase< Vector3d > &, const MatrixBase< Vector3d > &, MatrixBase< Vector3d > &)

Function Documentation

void angleDiff	(	MatrixBase< DerivedPhi1 > const &	phi1,
		MatrixBase< DerivedPhi2 > const &	phi2,
		MatrixBase< DerivedD > &	d
	)

Here is the caller graph for this function:

template DRAKECONTROLUTIL_EXPORT void angleDiff	(	const MatrixBase< MatrixXd > &	,
		const MatrixBase< MatrixXd > &	,
		MatrixBase< MatrixXd > &
	)

template DRAKECONTROLUTIL_EXPORT void angleDiff	(	const MatrixBase< Vector3d > &	,
		const MatrixBase< Vector3d > &	,
		MatrixBase< Vector3d > &
	)

Vector6d bodySpatialMotionPD	(	const RigidBodyTree &	r,
		const DrakeRobotState &	robot_state,
		const int	body_index,
		const Isometry3d &	body_pose_des,
		const Ref< const Vector6d > &	body_v_des,
		const Ref< const Vector6d > &	body_vdot_des,
		const Ref< const Vector6d > &	Kp,
		const Ref< const Vector6d > &	Kd,
		const Isometry3d &	T_task_to_world
	)

Here is the call graph for this function:

Here is the caller graph for this function:

int contactConstraintsBV	(	const RigidBodyTree &	r,
		const KinematicsCache< double > &	cache,
		int	nc,
		std::vector< double >	support_mus,
		std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &	supp,
		MatrixXd &	B,
		MatrixXd &	JB,
		MatrixXd &	Jp,
		VectorXd &	Jpdotv,
		MatrixXd &	normals
	)

Here is the call graph for this function:

Here is the caller graph for this function:

int contactPhi	(	const RigidBodyTree &	r,
		const KinematicsCache< double > &	cache,
		SupportStateElement &	supp,
		VectorXd &	phi
	)

Here is the call graph for this function:

Here is the caller graph for this function:

void evaluateXYZExpmapCubicSpline	(	double	t,
		const PiecewisePolynomial< double > &	spline,
		Isometry3d &	body_pose_des,
		Vector6d &	xyzdot_angular_vel,
		Vector6d &	xyzddot_angular_accel
	)

Here is the call graph for this function:

Here is the caller graph for this function:

Matrix<bool, Dynamic, 1> getActiveSupportMask	(	const RigidBodyTree &	r,
		VectorXd	q,
		VectorXd	qd,
		std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &	available_supports,
		const Ref< const Matrix< bool, Dynamic, 1 >> &	contact_force_detected,
		double	contact_threshold
	)

Here is the call graph for this function:

Here is the caller graph for this function:

std::vector<SupportStateElement, Eigen::aligned_allocator<SupportStateElement> > getActiveSupports	(	const RigidBodyTree &	r,
		const VectorXd &	q,
		const VectorXd &	qd,
		std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &	available_supports,
		const Ref< const Matrix< bool, Dynamic, 1 >> &	contact_force_detected,
		double	contact_threshold
	)

Here is the call graph for this function:

Here is the caller graph for this function:

void getCols	(	std::set< int > &	cols,
		MatrixBase< DerivedA > const &	M,
		MatrixBase< DerivedB > &	Msub
	)

Here is the caller graph for this function:

template DRAKECONTROLUTIL_EXPORT void getCols	(	std::set< int > &	,
		const MatrixBase< MatrixXd > &	,
		MatrixBase< MatrixXd > &
	)

void getRobotJointIndexMap	(	JointNames *	joint_names,
		RobotJointIndexMap *	joint_map
	)

Here is the call graph for this function:

void getRows	(	std::set< int > &	rows,
		MatrixBase< DerivedA > const &	M,
		MatrixBase< DerivedB > &	Msub
	)

Here is the caller graph for this function:

template DRAKECONTROLUTIL_EXPORT void getRows	(	std::set< int > &	,
		const MatrixBase< MatrixXd > &	,
		MatrixBase< MatrixXd > &
	)

MatrixXd individualSupportCOPs	(	const RigidBodyTree &	r,
		const KinematicsCache< double > &	cache,
		const std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &	active_supports,
		const MatrixXd &	normals,
		const MatrixXd &	B,
		const VectorXd &	beta
	)

Here is the call graph for this function:

bool inSupport	(	const std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &	supports,
		int	body_idx
	)

Here is the caller graph for this function:

bool isSupportElementActive	(	SupportStateElement *	se,
		bool	contact_force_detected,
		bool	kinematic_contact_detected
	)

Here is the caller graph for this function:

void surfaceTangents	(	const Vector3d &	normal,
		Matrix< double, 3, m_surface_tangents > &	d
	)

Here is the call graph for this function:

Here is the caller graph for this function:

Functions

Function Documentation