The main functionality class, which provides functionalities for evaluating robot environment information. More...

#include <robot_env_evaluator.hpp>

Public Member Functions
	RobotEnvEvaluator (const pinocchio::Model &model, const std::string &ee_name, const std::vector< std::string > &joint_names, const pinocchio::GeometryModel &collision_model, const pinocchio::GeometryModel &visual_model=pinocchio::GeometryModel())
	Construct a new Robot Env Evaluator object.

	~RobotEnvEvaluator ()=default
	Destroy the Robot Env Evaluator object.

void	forwardKinematics (const Eigen::VectorXd &q, Eigen::Matrix4d &T, const int joint_index=-1)
	The forward kinematics function for a specific joint, specified by the index.

void	forwardKinematicsByFrameName (const Eigen::VectorXd &q, Eigen::Matrix4d &T, const std::string &frame_name)
	The forward kinematics function for a specific frame, specified by the name.

void	jacobian (const Eigen::VectorXd &q, Eigen::MatrixXd &J, const int joint_index=-1)
	The jacobian function for a specific joint, specified by the index.

void	jacobianByFrameName (const Eigen::VectorXd &q, Eigen::MatrixXd &J, const std::string &frame_name)
	Get the jacobian of specific frame, specified by the name.

void	computeDistances (const Eigen::VectorXd &q, const std::vector< obstacleInput > &obstacles, std::vector< distanceResult > &distances)
	The distance computation function between the robot and obstacles.

Public Attributes
bool	calculate_self_collision_ = false
	Whether to calculate self-collision in distance computation.

bool	projector_dist_to_control_enable_ = false
	Whether to calculate the projector from distance to control space.

bool	projector_dist_to_control_with_zero_orientation_ = false
	Whether to use the full Jacobian and set the orientation to zero when calculating the projector.

double	robust_pinv_lambda_ = 0.001
	The lambda value for the robust pseudo-inverse projector.

Protected Member Functions
void	jacobianFrame (const Eigen::VectorXd &q, const double frame_index, Eigen::MatrixXd &J)
	Get the Jacobian of specific frame.

void	computeModelData (const Eigen::VectorXd &q)
	Compute the model data from the joint configuration.

Protected Attributes
Eigen::VectorXd	buffered_q_
	The buffered joint configuration. This is used to avoid unnecessary computation.

pinocchio::Model	model_
	The robot model.

pinocchio::Data	data_
	The robot data, initialized from the model.

pinocchio::GeometryModel	collision_model_
	The collision model.

pinocchio::GeometryModel	visual_model_
	The visual model.

int	ee_index_
	The internal index for end-effector.frame.

std::vector< double >	joint_indices_
	The joint indices for the robot model.

pinocchio::GeometryModel	geom_model_
	The geometry model for collision computation.

pinocchio::GeometryData	geom_data_
	The geometry data for collision computation.

Detailed Description

The main functionality class, which provides functionalities for evaluating robot environment information.

This class provides methods for forward kinematics, jacobian computation, and distance computation between the robot and obstacles in the environment. It uses the Pinocchio library for kinematics, the pinocchio's submodule coal for collision detection.

The initialization:

Use RobotPresetFactory::createRobotPreset() in robot_presets.hpp to create a robot model, which includes the robot's kinematic model [pinocchio::Model], end-effector name, joint names, and [pinocchio::GeometryModel] collision model. Or, you can create your own robot model by loading from URDF files with pinocchio's urdf parser.
Create a RobotEnvEvaluator object with the above data.

The functionalities include:

Forward kinematics for a specific joint or frame: forwardKinematics() and forwardKinematicsByFrameName()
Jacobian computation for a specific joint or frame: jacobian() and jacobianByFrameName()
Distance computation between the robot and obstacles: computeDistances()

Examples: robot_env_evaluator_example.cpp.

Definition at line 82 of file robot_env_evaluator.hpp.

Constructor & Destructor Documentation

◆ RobotEnvEvaluator()

robot_env_evaluator::RobotEnvEvaluator::RobotEnvEvaluator	(	const pinocchio::Model &	model,
		const std::string &	ee_name,
		const std::vector< std::string > &	joint_names,
		const pinocchio::GeometryModel &	collision_model,
		const pinocchio::GeometryModel &	visual_model = `pinocchio::GeometryModel()`
	)

Construct a new Robot Env Evaluator object.

Parameters

[in]	model	The robot model
[in]	ee_name	The end-effector name
[in]	joint_names	The joint names
[in]	collision_model	The collision model
[in]	visual_model	The visual model

These models are supposed to be loaded from URDF files, and then activate (also deactivate) the collision pairs. Also editing model is acceptable, therefore this class allows a flexible way to input them.

Definition at line 28 of file robot_env_evaluator.cpp.

Member Function Documentation

◆ computeDistances()

void robot_env_evaluator::RobotEnvEvaluator::computeDistances	(	const Eigen::VectorXd &	q,
		const std::vector< obstacleInput > &	obstacles,
		std::vector< distanceResult > &	distances
	)

The distance computation function between the robot and obstacles.

Parameters

[in]	q	The joint configuration
[in]	obstacles	The obstacles in the world, including the shape and pose
[out]	distances	The distances output between the robot and obstacles

Examples: robot_env_evaluator_example.cpp.

Definition at line 123 of file robot_env_evaluator.cpp.

◆ computeModelData()

void robot_env_evaluator::RobotEnvEvaluator::computeModelData ( const Eigen::VectorXd & q )

protected

Compute the model data from the joint configuration.

Parameters

[in] q The joint configuration

This function is used to check if the joint configuration is changed, if yes, the computation will be done.

Definition at line 226 of file robot_env_evaluator.cpp.

◆ forwardKinematics()

void robot_env_evaluator::RobotEnvEvaluator::forwardKinematics	(	const Eigen::VectorXd &	q,
		Eigen::Matrix4d &	T,
		const int	joint_index = `-1`
	)

The forward kinematics function for a specific joint, specified by the index.

Parameters

[in]	q	The joint configuration
[out]	T	The transformation matrix of the joint
[in]	joint_index	The joint index

As pinocchio convention, the index 0 is the base joint, and the index 1 is the first joint, therefore 1 to length of [joint_names] in input is the real joint index. Default is -1, which means the end-effector.

Definition at line 56 of file robot_env_evaluator.cpp.

◆ forwardKinematicsByFrameName()

void robot_env_evaluator::RobotEnvEvaluator::forwardKinematicsByFrameName	(	const Eigen::VectorXd &	q,
		Eigen::Matrix4d &	T,
		const std::string &	frame_name
	)

The forward kinematics function for a specific frame, specified by the name.

Parameters

[in]	q	The joint configuration
[out]	T	The transformation matrix of the joint
[in]	frame_name	The frame name, could be joint or link

The name will the defined by the robot URDF file.

Definition at line 73 of file robot_env_evaluator.cpp.

◆ jacobian()

void robot_env_evaluator::RobotEnvEvaluator::jacobian	(	const Eigen::VectorXd &	q,
		Eigen::MatrixXd &	J,
		const int	joint_index = `-1`
	)

The jacobian function for a specific joint, specified by the index.

Parameters

[in]	q	The joint configuration
[out]	J	The jacobian matrix of the joint
[in]	joint_index	The joint index

As pinocchio convention, the index 0 is the base joint, and the index 1 is the first joint, therefore 1 to length of [joint_names] in input is the real joint index. Default is -1, which means the end-effector.

Examples: robot_env_evaluator_example.cpp.

Definition at line 86 of file robot_env_evaluator.cpp.

◆ jacobianByFrameName()

void robot_env_evaluator::RobotEnvEvaluator::jacobianByFrameName	(	const Eigen::VectorXd &	q,
		Eigen::MatrixXd &	J,
		const std::string &	frame_name
	)

Get the jacobian of specific frame, specified by the name.

Parameters

[in]	q	The joint configuration
[out]	J	The jacobian matrix of the joint
[in]	frame_name	The frame name, could be joint or link

Definition at line 102 of file robot_env_evaluator.cpp.

◆ jacobianFrame()

void robot_env_evaluator::RobotEnvEvaluator::jacobianFrame	(	const Eigen::VectorXd &	q,
		const double	frame_index,
		Eigen::MatrixXd &	J
	)

protected

Get the Jacobian of specific frame.

Parameters

[in]	q	The joint configuration
[in]	frame_index	The frame index
[out]	J	The output jacobian matrix

Warning, direct use of this function is not recommended because it does not apply any safety check on the frame index. Use jacobianByFrameName() instead.

Definition at line 115 of file robot_env_evaluator.cpp.

Member Data Documentation

◆ buffered_q_

Eigen::VectorXd robot_env_evaluator::RobotEnvEvaluator::buffered_q_

protected

The buffered joint configuration. This is used to avoid unnecessary computation.

Definition at line 204 of file robot_env_evaluator.hpp.

◆ calculate_self_collision_

bool robot_env_evaluator::RobotEnvEvaluator::calculate_self_collision_ = false

Whether to calculate self-collision in distance computation.

Definition at line 173 of file robot_env_evaluator.hpp.

◆ collision_model_

pinocchio::GeometryModel robot_env_evaluator::RobotEnvEvaluator::collision_model_

protected

The collision model.

Definition at line 208 of file robot_env_evaluator.hpp.

◆ data_

pinocchio::Data robot_env_evaluator::RobotEnvEvaluator::data_

protected

The robot data, initialized from the model.

Definition at line 207 of file robot_env_evaluator.hpp.

◆ ee_index_

int robot_env_evaluator::RobotEnvEvaluator::ee_index_

protected

The internal index for end-effector.frame.

Definition at line 210 of file robot_env_evaluator.hpp.

◆ geom_data_

pinocchio::GeometryData robot_env_evaluator::RobotEnvEvaluator::geom_data_

protected

The geometry data for collision computation.

Definition at line 214 of file robot_env_evaluator.hpp.

◆ geom_model_

pinocchio::GeometryModel robot_env_evaluator::RobotEnvEvaluator::geom_model_

protected

The geometry model for collision computation.

Definition at line 213 of file robot_env_evaluator.hpp.

◆ joint_indices_

std::vector<double> robot_env_evaluator::RobotEnvEvaluator::joint_indices_

protected

The joint indices for the robot model.

Definition at line 211 of file robot_env_evaluator.hpp.

◆ model_

pinocchio::Model robot_env_evaluator::RobotEnvEvaluator::model_

protected

The robot model.

Definition at line 206 of file robot_env_evaluator.hpp.

◆ projector_dist_to_control_enable_

bool robot_env_evaluator::RobotEnvEvaluator::projector_dist_to_control_enable_ = false

Whether to calculate the projector from distance to control space.

Definition at line 174 of file robot_env_evaluator.hpp.

◆ projector_dist_to_control_with_zero_orientation_

bool robot_env_evaluator::RobotEnvEvaluator::projector_dist_to_control_with_zero_orientation_ = false

Whether to use the full Jacobian and set the orientation to zero when calculating the projector.

Definition at line 175 of file robot_env_evaluator.hpp.

◆ robust_pinv_lambda_

double robot_env_evaluator::RobotEnvEvaluator::robust_pinv_lambda_ = 0.001

The lambda value for the robust pseudo-inverse projector.

Definition at line 176 of file robot_env_evaluator.hpp.

◆ visual_model_

pinocchio::GeometryModel robot_env_evaluator::RobotEnvEvaluator::visual_model_

protected

The visual model.

Definition at line 209 of file robot_env_evaluator.hpp.

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

include/robot_env_evaluator/robot_env_evaluator.hpp
src/robot_env_evaluator.cpp

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ RobotEnvEvaluator()

Member Function Documentation

◆ computeDistances()

◆ computeModelData()

◆ forwardKinematics()

◆ forwardKinematicsByFrameName()

◆ jacobian()

◆ jacobianByFrameName()

◆ jacobianFrame()

Member Data Documentation

◆ buffered_q_

◆ calculate_self_collision_

◆ collision_model_

◆ data_

◆ ee_index_

◆ geom_data_

◆ geom_model_

◆ joint_indices_

◆ model_

◆ projector_dist_to_control_enable_

◆ projector_dist_to_control_with_zero_orientation_

◆ robust_pinv_lambda_

◆ visual_model_