Equality constraints on a link's position. More...

#include <ompl_constraints.h>

Inheritance diagram for ompl_interface::EqualityPositionConstraint:

[legend]

Collaboration diagram for ompl_interface::EqualityPositionConstraint:

[legend]

Public Member Functions
	EqualityPositionConstraint (const moveit::core::RobotModelConstPtr &robot_model, const std::string &group, const unsigned int num_dofs)

void	parseConstraintMsg (const moveit_msgs::msg::Constraints &constraints) override
	Parse bounds on position parameters from MoveIt's constraint message.

void	function (const Eigen::Ref< const Eigen::VectorXd > &joint_values, Eigen::Ref< Eigen::VectorXd > out) const override

void	jacobian (const Eigen::Ref< const Eigen::VectorXd > &joint_values, Eigen::Ref< Eigen::MatrixXd > out) const override

Public Member Functions inherited from ompl_interface::BaseConstraint
	BaseConstraint (const moveit::core::RobotModelConstPtr &robot_model, const std::string &group, const unsigned int num_dofs, const unsigned int num_cons=3)
	Construct a BaseConstraint using 3 `num_cons` by default because all constraints currently implemented have 3 constraint equations.

void	init (const moveit_msgs::msg::Constraints &constraints)
	Initialize constraint based on message content.

void	function (const Eigen::Ref< const Eigen::VectorXd > &joint_values, Eigen::Ref< Eigen::VectorXd > out) const override

void	jacobian (const Eigen::Ref< const Eigen::VectorXd > &joint_values, Eigen::Ref< Eigen::MatrixXd > out) const override
	Jacobian of the constraint function.

Eigen::Isometry3d	forwardKinematics (const Eigen::Ref< const Eigen::VectorXd > &joint_values) const
	Wrapper for forward kinematics calculated by MoveIt's Robot State.

Eigen::MatrixXd	robotGeometricJacobian (const Eigen::Ref< const Eigen::VectorXd > &joint_values) const
	Calculate the robot's geometric Jacobian using MoveIt's Robot State.

virtual Eigen::VectorXd	calcError (const Eigen::Ref< const Eigen::VectorXd > &) const
	For inequality constraints: calculate the value of the parameter that is being constrained by the bounds.

virtual Eigen::MatrixXd	calcErrorJacobian (const Eigen::Ref< const Eigen::VectorXd > &) const
	For inequality constraints: calculate the Jacobian for the current parameters that are being constrained. This error jacobian, as the name suggests, is only the jacobian of the position / orientation / ... error. It does not take into account the derivative of the penalty functions defined in the Bounds class. This correction is added in the implementation of of BaseConstraint::jacobian.

const std::string &	getLinkName ()

const Eigen::Vector3d	getTargetPosition ()

const Eigen::Quaterniond	getTargetOrientation ()

Additional Inherited Members
Protected Attributes inherited from ompl_interface::BaseConstraint
TSStateStorage	state_storage_
	Thread-safe storage of the robot state.

const moveit::core::JointModelGroup *	joint_model_group_

std::string	link_name_
	Robot link the constraints are applied to.

Bounds	bounds_
	Upper and lower bounds on constrained variables.

Eigen::Vector3d	target_position_
	target for equality constraints, nominal value for inequality constraints.

Eigen::Quaterniond	target_orientation_
	target for equality constraints, nominal value for inequality constraints.

Detailed Description

Equality constraints on a link's position.

When you set the name of a constraint to 'use_equality_constraints', all constraints with a dimension lower than EQUALITY_CONSTRAINT_THRESHOLD will be modelled as equality constraints.

The dimension value for the others are ignored. For example, a box with dimensions [1.0, 1e-5, 1.0] will result in equality constraints on the y-position, and no constraints on the x or z-position.

TODO(jeroendm) We could make this a base class EqualityConstraints with a specialization for position and orientation constraints in the future. But the direct overriding of function and jacobian is probably more performant.

Definition at line 309 of file ompl_constraints.h.

Constructor & Destructor Documentation

◆ EqualityPositionConstraint()

ompl_interface::EqualityPositionConstraint::EqualityPositionConstraint	(	const moveit::core::RobotModelConstPtr &	robot_model,
		const std::string &	group,
		const unsigned int	num_dofs
	)

Definition at line 234 of file ompl_constraints.cpp.

Member Function Documentation

◆ function()

void ompl_interface::EqualityPositionConstraint::function	(	const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		Eigen::Ref< Eigen::VectorXd >	out
	)		const

override

Definition at line 275 of file ompl_constraints.cpp.

Here is the call graph for this function:

◆ jacobian()

void ompl_interface::EqualityPositionConstraint::jacobian	(	const Eigen::Ref< const Eigen::VectorXd > &	joint_values,
		Eigen::Ref< Eigen::MatrixXd >	out
	)		const

override

Definition at line 293 of file ompl_constraints.cpp.

Here is the call graph for this function:

◆ parseConstraintMsg()

void ompl_interface::EqualityPositionConstraint::parseConstraintMsg ( const moveit_msgs::msg::Constraints & constraints )

overridevirtual

Parse bounds on position parameters from MoveIt's constraint message.

This can be non-trivial given the often complex structure of these messages.

Implements ompl_interface::BaseConstraint.

Definition at line 240 of file ompl_constraints.cpp.

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

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h
moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp

Public Member Functions