controlUtil.h File Reference

#include <math.h>
#include <set>
#include <vector>
#include <Eigen/Dense>
#include <Eigen/StdVector>
#include "drake/systems/plants/RigidBodyTree.h"
#include "drake/systems/trajectories/PiecewisePolynomial.h"
#include "drake/drakeControlUtil_export.h"

Include dependency graph for controlUtil.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
struct	SupportStateElement
struct	DrakeRobotState
struct	RobotJointIndexMap
struct	JointNames

Macros
#define	EPSILON   10e-8

Typedefs
typedef Eigen::Matrix< double, 6, 1 >	Vector6d
typedef Eigen::Matrix< double, 7, 1 >	Vector7d

Functions
DRAKECONTROLUTIL_EXPORT bool	isSupportElementActive (SupportStateElement *se, bool contact_force_detected, bool kinematic_contact_detected)
DRAKECONTROLUTIL_EXPORT Eigen::Matrix< bool, Eigen::Dynamic, 1 >	getActiveSupportMask (RigidBodyTree *r, Eigen::VectorXd q, Eigen::VectorXd qd, std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &available_supports, const Eigen::Ref< const Eigen::Matrix< bool, Eigen::Dynamic, 1 >> &contact_force_detected, double contact_threshold)
DRAKECONTROLUTIL_EXPORT std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement > >	getActiveSupports (const RigidBodyTree &r, const Eigen::VectorXd &q, const Eigen::VectorXd &qd, std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &available_supports, const Eigen::Ref< const Eigen::Matrix< bool, Eigen::Dynamic, 1 >> &contact_force_detected, double contact_threshold)
template<typename DerivedA , typename DerivedB >
DRAKECONTROLUTIL_EXPORT void	getRows (std::set< int > &rows, Eigen::MatrixBase< DerivedA > const &M, Eigen::MatrixBase< DerivedB > &Msub)
template<typename DerivedA , typename DerivedB >
DRAKECONTROLUTIL_EXPORT void	getCols (std::set< int > &cols, Eigen::MatrixBase< DerivedA > const &M, Eigen::MatrixBase< DerivedB > &Msub)
template<typename DerivedPhi1 , typename DerivedPhi2 , typename DerivedD >
DRAKECONTROLUTIL_EXPORT void	angleDiff (Eigen::MatrixBase< DerivedPhi1 > const &phi1, Eigen::MatrixBase< DerivedPhi2 > const &phi2, Eigen::MatrixBase< DerivedD > &d)
DRAKECONTROLUTIL_EXPORT bool	inSupport (const std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &supports, int body_idx)
DRAKECONTROLUTIL_EXPORT void	surfaceTangents (const Eigen::Vector3d &normal, Eigen::Matrix< double, 3, m_surface_tangents > &d)
DRAKECONTROLUTIL_EXPORT int	contactPhi (const RigidBodyTree &r, const KinematicsCache< double > &cache, SupportStateElement &supp, Eigen::VectorXd &phi)
DRAKECONTROLUTIL_EXPORT int	contactConstraintsBV (const RigidBodyTree &r, const KinematicsCache< double > &cache, int nc, std::vector< double > support_mus, std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &supp, Eigen::MatrixXd &B, Eigen::MatrixXd &JB, Eigen::MatrixXd &Jp, Eigen::VectorXd &Jpdotv, Eigen::MatrixXd &normals)
DRAKECONTROLUTIL_EXPORT Eigen::MatrixXd	individualSupportCOPs (const RigidBodyTree &r, const KinematicsCache< double > &cache, const std::vector< SupportStateElement, Eigen::aligned_allocator< SupportStateElement >> &active_supports, const Eigen::MatrixXd &normals, const Eigen::MatrixXd &B, const Eigen::VectorXd &beta)
DRAKECONTROLUTIL_EXPORT Vector6d	bodySpatialMotionPD (const RigidBodyTree &r, const DrakeRobotState &robot_state, const int body_index, const Eigen::Isometry3d &body_pose_des, const Eigen::Ref< const Vector6d > &body_v_des, const Eigen::Ref< const Vector6d > &body_vdot_des, const Eigen::Ref< const Vector6d > &Kp, const Eigen::Ref< const Vector6d > &Kd, const Eigen::Isometry3d &T_task_to_world=Eigen::Isometry3d::Identity())
DRAKECONTROLUTIL_EXPORT void	evaluateXYZExpmapCubicSpline (double t, const PiecewisePolynomial< double > &spline, Eigen::Isometry3d &body_pose_des, Vector6d &xyzdot_angular_vel, Vector6d &xyzddot_angular_accel)
DRAKECONTROLUTIL_EXPORT void	getRobotJointIndexMap (JointNames *joint_names, RobotJointIndexMap *joint_map)

Variables
const int	m_surface_tangents

Macro Definition Documentation

#define EPSILON   10e-8

Typedef Documentation

typedef Eigen::Matrix<double, 6, 1> Vector6d

typedef Eigen::Matrix<double, 7, 1> Vector7d

Function Documentation

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

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

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

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

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

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

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

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

DRAKECONTROLUTIL_EXPORT void getRobotJointIndexMap	(	JointNames *	joint_names,
		RobotJointIndexMap *	joint_map
	)

Here is the call graph for this function:

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

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

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

Here is the caller graph for this function:

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

Here is the caller graph for this function:

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

Variable Documentation

const int m_surface_tangents

Initial value:

=
    2