Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
stomp_moveit::cost_functions::CollisionCheck Class Reference

Assigns a cost value to each robot state by evaluating if the robot is in collision. More...

#include <collision_check.h>

Inheritance diagram for stomp_moveit::cost_functions::CollisionCheck:
Inheritance graph
[legend]

Public Member Functions

virtual bool computeCosts (const Eigen::MatrixXd &parameters, std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::VectorXd &costs, bool &validity) override
 computes the state costs by checking whether the robot is in collision at each time step. More...
 
virtual bool configure (const XmlRpc::XmlRpcValue &config) override
 Sets internal members of the plugin from the configuration data. More...
 
virtual void done (bool success, int total_iterations, double final_cost, const Eigen::MatrixXd &parameters) override
 Called by the Stomp Task at the end of the optimization process. More...
 
virtual std::string getGroupName () const override
 
virtual std::string getName () const override
 
virtual bool initialize (moveit::core::RobotModelConstPtr robot_model_ptr, const std::string &group_name, XmlRpc::XmlRpcValue &config) override
 Initializes and configures the Cost Function. Calls the configure method and passes the 'config' value. More...
 
virtual bool setMotionPlanRequest (const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::MotionPlanRequest &req, const stomp_core::StompConfiguration &config, moveit_msgs::MoveItErrorCodes &error_code) override
 Stores the planning details which will be used during the costs calculations. More...
 
- Public Member Functions inherited from stomp_moveit::cost_functions::StompCostFunction
virtual int getOptimizedIndex () const
 The index returned by this method will be passed by the Task to the corresponding plugin method as the 'rollout_number' argument when operating on the noiseless (optimized) parameters.
 
virtual double getWeight () const
 
virtual void postIteration (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, double cost, const Eigen::MatrixXd &parameters)
 Called by STOMP at the end of each iteration. More...
 

Protected Member Functions

bool checkIntermediateCollisions (const Eigen::VectorXd &start, const Eigen::VectorXd &end, double longest_valid_joint_move)
 Checks for collision between consecutive points by dividing the joint move into sub-moves where the maximum joint motion can not exceed the longest_valid_joint_move value. More...
 

Protected Attributes

double collision_penalty_
 The value assigned to a collision state.
 
std::string group_name_
 
std::array< moveit::core::RobotStatePtr, 3 > intermediate_coll_states_
 Used in checking collisions between to consecutive poses.
 
Eigen::ArrayXd intermediate_costs_slots_
 
double kernel_window_percentage_
 The value assigned to a collision state.
 
double longest_valid_joint_move_
 how far can a joint move in between consecutive trajectory points
 
std::string name_
 
moveit_msgs::MotionPlanRequest plan_request_
 
planning_scene::PlanningSceneConstPtr planning_scene_
 
Eigen::VectorXd raw_costs_
 
moveit::core::RobotModelConstPtr robot_model_ptr_
 
moveit::core::RobotStatePtr robot_state_
 
- Protected Attributes inherited from stomp_moveit::cost_functions::StompCostFunction
double cost_weight_
 

Detailed Description

Assigns a cost value to each robot state by evaluating if the robot is in collision.

Examples:
All examples are located here STOMP MoveIt! examples

Definition at line 45 of file collision_check.h.

Member Function Documentation

bool stomp_moveit::cost_functions::CollisionCheck::checkIntermediateCollisions ( const Eigen::VectorXd &  start,
const Eigen::VectorXd &  end,
double  longest_valid_joint_move 
)
protected

Checks for collision between consecutive points by dividing the joint move into sub-moves where the maximum joint motion can not exceed the longest_valid_joint_move value.

Parameters
startThe start joint pose
endThe end joint pose
longest_valid_joint_moveThe maximum distance that the joints are allowed to move before checking for collisions.
Returns
True if the interval is collision free, false otherwise.

Definition at line 279 of file collision_check.cpp.

bool stomp_moveit::cost_functions::CollisionCheck::computeCosts ( const Eigen::MatrixXd &  parameters,
std::size_t  start_timestep,
std::size_t  num_timesteps,
int  iteration_number,
int  rollout_number,
Eigen::VectorXd &  costs,
bool &  validity 
)
overridevirtual

computes the state costs by checking whether the robot is in collision at each time step.

Parameters
parametersThe parameter values to evaluate for state costs [num_dimensions x num_parameters]
start_timestepstart index into the 'parameters' array, usually 0.
num_timestepsnumber of elements to use from 'parameters' starting from 'start_timestep' *
iteration_numberThe current iteration count in the optimization loop
rollout_numberindex of the noisy trajectory whose cost is being evaluated. *
costsvector containing the state costs per timestep. Sets '0' to all collision-free states.
validitywhether or not the trajectory is valid.
Returns
false if there was an irrecoverable failure, true otherwise.

Implements stomp_moveit::cost_functions::StompCostFunction.

Definition at line 180 of file collision_check.cpp.

bool stomp_moveit::cost_functions::CollisionCheck::configure ( const XmlRpc::XmlRpcValue config)
overridevirtual

Sets internal members of the plugin from the configuration data.

Parameters
configThe configuration data. Usually loaded from the ros parameter server
Returns
true if succeeded, false otherwise.

Implements stomp_moveit::cost_functions::StompCostFunction.

Definition at line 322 of file collision_check.cpp.

void stomp_moveit::cost_functions::CollisionCheck::done ( bool  success,
int  total_iterations,
double  final_cost,
const Eigen::MatrixXd &  parameters 
)
overridevirtual

Called by the Stomp Task at the end of the optimization process.

Parameters
successWhether the optimization succeeded
total_iterationsNumber of iterations used
final_costThe cost value after optimizing.
parametersThe parameters generated at the end of current iteration [num_dimensions x num_timesteps]

Reimplemented from stomp_moveit::cost_functions::StompCostFunction.

Definition at line 364 of file collision_check.cpp.

bool stomp_moveit::cost_functions::CollisionCheck::initialize ( moveit::core::RobotModelConstPtr  robot_model_ptr,
const std::string &  group_name,
XmlRpc::XmlRpcValue config 
)
overridevirtual

Initializes and configures the Cost Function. Calls the configure method and passes the 'config' value.

Parameters
robot_model_ptrA pointer to the robot model.
group_nameThe designated planning group.
configThe configuration data. Usually loaded from the ros parameter server
Returns
true if succeeded, false otherwise.

Implements stomp_moveit::cost_functions::StompCostFunction.

Definition at line 140 of file collision_check.cpp.

bool stomp_moveit::cost_functions::CollisionCheck::setMotionPlanRequest ( const planning_scene::PlanningSceneConstPtr &  planning_scene,
const moveit_msgs::MotionPlanRequest req,
const stomp_core::StompConfiguration config,
moveit_msgs::MoveItErrorCodes &  error_code 
)
overridevirtual

Stores the planning details which will be used during the costs calculations.

Parameters
planning_sceneA smart pointer to the planning scene
reqThe motion planning request
configThe Stomp configuration.
error_codeMoveit error code.
Returns
true if succeeded, false otherwise.

Implements stomp_moveit::cost_functions::StompCostFunction.

Definition at line 148 of file collision_check.cpp.


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


stomp_moveit
Author(s): Jorge Nicho
autogenerated on Fri May 8 2020 03:35:47