Checks that the joint values are within the limits as defined in the urdf file. It modifies the values of those joints that exceed the limits. More...
#include <joint_limits.h>
Public Member Functions | |
virtual bool | configure (const XmlRpc::XmlRpcValue &config) override |
see base class for documentation | |
virtual bool | filter (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, int rollout_number, Eigen::MatrixXd ¶meters, bool &filtered) override |
Sets the joint values to the closest joint limit whenever it's found outside the allowed range. | |
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, const XmlRpc::XmlRpcValue &config) override |
see base class for documentation | |
JointLimits () | |
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 |
see base class for documentation | |
virtual | ~JointLimits () |
Protected Attributes | |
moveit::core::RobotStatePtr | goal_state_ |
std::string | group_name_ |
bool | lock_goal_ |
bool | lock_start_ |
moveit::core::RobotModelConstPtr | robot_model_ |
moveit::core::RobotStatePtr | start_state_ |
Checks that the joint values are within the limits as defined in the urdf file. It modifies the values of those joints that exceed the limits.
Definition at line 46 of file joint_limits.h.
Definition at line 38 of file joint_limits.cpp.
stomp_moveit::noisy_filters::JointLimits::~JointLimits | ( | ) | [virtual] |
Definition at line 46 of file joint_limits.cpp.
bool stomp_moveit::noisy_filters::JointLimits::configure | ( | const XmlRpc::XmlRpcValue & | config | ) | [override, virtual] |
see base class for documentation
Implements stomp_moveit::noisy_filters::StompNoisyFilter.
Definition at line 66 of file joint_limits.cpp.
bool stomp_moveit::noisy_filters::JointLimits::filter | ( | std::size_t | start_timestep, |
std::size_t | num_timesteps, | ||
int | iteration_number, | ||
int | rollout_number, | ||
Eigen::MatrixXd & | parameters, | ||
bool & | filtered | ||
) | [override, virtual] |
Sets the joint values to the closest joint limit whenever it's found outside the allowed range.
start_timestep | Start index into the 'parameters' array, usually 0. |
num_timesteps | Number of elements to use from 'parameters' starting from 'start_timestep' |
iteration_number | The current iteration count in the optimization loop. |
rollout_number | Index of the noisy trajectory whose cost is being evaluated. |
parameters | Output argument containing the parameters to be filtered [num_dimensions x num_timesteps]. |
filtered | Output argument that's set to 'true' if the parameters were changed according to the filtering method. |
Implements stomp_moveit::noisy_filters::StompNoisyFilter.
Definition at line 146 of file joint_limits.cpp.
virtual std::string stomp_moveit::noisy_filters::JointLimits::getGroupName | ( | ) | const [inline, override, virtual] |
Reimplemented from stomp_moveit::noisy_filters::StompNoisyFilter.
Definition at line 66 of file joint_limits.h.
virtual std::string stomp_moveit::noisy_filters::JointLimits::getName | ( | ) | const [inline, override, virtual] |
Reimplemented from stomp_moveit::noisy_filters::StompNoisyFilter.
Definition at line 71 of file joint_limits.h.
bool stomp_moveit::noisy_filters::JointLimits::initialize | ( | moveit::core::RobotModelConstPtr | robot_model_ptr, |
const std::string & | group_name, | ||
const XmlRpc::XmlRpcValue & | config | ||
) | [override, virtual] |
see base class for documentation
Implements stomp_moveit::noisy_filters::StompNoisyFilter.
Definition at line 51 of file joint_limits.cpp.
bool stomp_moveit::noisy_filters::JointLimits::setMotionPlanRequest | ( | const planning_scene::PlanningSceneConstPtr & | planning_scene, |
const moveit_msgs::MotionPlanRequest & | req, | ||
const stomp_core::StompConfiguration & | config, | ||
moveit_msgs::MoveItErrorCodes & | error_code | ||
) | [override, virtual] |
see base class for documentation
Implements stomp_moveit::noisy_filters::StompNoisyFilter.
Definition at line 95 of file joint_limits.cpp.
moveit::core::RobotStatePtr stomp_moveit::noisy_filters::JointLimits::goal_state_ [protected] |
Definition at line 105 of file joint_limits.h.
std::string stomp_moveit::noisy_filters::JointLimits::group_name_ [protected] |
Definition at line 97 of file joint_limits.h.
bool stomp_moveit::noisy_filters::JointLimits::lock_goal_ [protected] |
Definition at line 101 of file joint_limits.h.
bool stomp_moveit::noisy_filters::JointLimits::lock_start_ [protected] |
Definition at line 100 of file joint_limits.h.
moveit::core::RobotModelConstPtr stomp_moveit::noisy_filters::JointLimits::robot_model_ [protected] |
Definition at line 96 of file joint_limits.h.
moveit::core::RobotStatePtr stomp_moveit::noisy_filters::JointLimits::start_state_ [protected] |
Definition at line 104 of file joint_limits.h.