Constrains an endeffector to lie in a box around the nominal stance. More...

#include <range_of_motion_constraint.h>

Inheritance diagram for towr::RangeOfMotionConstraint:

Public Member Functions
	RangeOfMotionConstraint (const KinematicModel::Ptr &robot_model, double T, double dt, const EE &ee, const SplineHolder &spline_holder)
	Constructs a constraint instance.
virtual	~RangeOfMotionConstraint ()
Private Member Functions
int	GetRow (int node, int dimension) const
void	UpdateBoundsAtInstance (double t, int k, VecBound &) const override
	Sets upper/lower bound a specific time t, corresponding to node k.
void	UpdateConstraintAtInstance (double t, int k, VectorXd &g) const override
	Sets the constraint value a specific time t, corresponding to node k.
void	UpdateJacobianAtInstance (double t, int k, std::string, Jacobian &) const override
	Sets Jacobian rows at a specific time t, corresponding to node k.
Private Attributes
EulerConverter	base_angular_
	the orientation of the base.
NodeSpline::Ptr	base_linear_
	the linear position of the base.
EE	ee_
NodeSpline::Ptr	ee_motion_
	the linear position of the endeffectors.
Eigen::Vector3d	max_deviation_from_nominal_
Eigen::Vector3d	nominal_ee_pos_B_

Detailed Description

Constrains an endeffector to lie in a box around the nominal stance.

These constraints are necessary to avoid configurations that are outside the kinematic reach of the robot. The constraint is defined by Cartesian estimates of the reachability of each endeffector.

This constraint calculates the position of of the contact expressed in the current CoM frame and constrains it to lie in a box around the nominal/ natural contact position for that leg.

Definition at line 55 of file range_of_motion_constraint.h.

Constructor & Destructor Documentation

towr::RangeOfMotionConstraint::RangeOfMotionConstraint	(	const KinematicModel::Ptr &	robot_model,
		double	T,
		double	dt,
		const EE &	ee,
		const SplineHolder &	spline_holder
	)

Constructs a constraint instance.

Parameters:

robot_model	The kinematic restrictions of the robot.
T	The total duration of the optimization.
dt	the discretization intervall at which to enforce constraints.
ee	The endeffector for which to constrain the range.
spline_holder	Pointer to the current variables.

Definition at line 35 of file range_of_motion_constraint.cc.

virtual towr::RangeOfMotionConstraint::~RangeOfMotionConstraint ( ) [virtual]

Member Function Documentation

int towr::RangeOfMotionConstraint::GetRow	(	int	node,
		int	dimension
	)		const `[private]`

Definition at line 53 of file range_of_motion_constraint.cc.

void towr::RangeOfMotionConstraint::UpdateBoundsAtInstance	(	double	t,
		int	k,
		VecBound &	b
	)		const `[override, private, virtual]`

Sets upper/lower bound a specific time t, corresponding to node k.

Parameters:

t	The time along the trajectory to set the bounds.
k	The index of the time t, so t=k*dt
in/out]	b The complete vector of bounds, for which the corresponding row must be set.

Implements towr::TimeDiscretizationConstraint.

Definition at line 72 of file range_of_motion_constraint.cc.

void towr::RangeOfMotionConstraint::UpdateConstraintAtInstance	(	double	t,
		int	k,
		VectorXd &	g
	)		const `[override, private, virtual]`

Sets the constraint value a specific time t, corresponding to node k.

Parameters:

t	The time along the trajectory to set the constraint.
k	The index of the time t, so t=k*dt
in/out]	g The complete vector of constraint values, for which the corresponding row must be filled.

Implements towr::TimeDiscretizationConstraint.

Definition at line 59 of file range_of_motion_constraint.cc.

void towr::RangeOfMotionConstraint::UpdateJacobianAtInstance	(	double	t,
		int	k,
		std::string	var_set,
		Jacobian &	jac
	)		const `[override, private, virtual]`

Sets Jacobian rows at a specific time t, corresponding to node k.

Parameters:

t	The time along the trajcetory to set the bounds.
k	The index of the time t, so t=k*dt
var_set	The name of the ifopt variables currently being queried for.
in/out]	jac The complete Jacobian, for which the corresponding row and columns must be set.

Implements towr::TimeDiscretizationConstraint.

Definition at line 84 of file range_of_motion_constraint.cc.