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. 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, const XmlRpc::XmlRpcValue &config) override |
see base class for documentation | |
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 | |
Public Member Functions inherited from stomp_moveit::noisy_filters::StompNoisyFilter | |
virtual void | done (bool success, int total_iterations, double final_cost, const Eigen::MatrixXd ¶meters) |
Called by the Stomp at the end of the optimization process. More... | |
virtual void | postIteration (std::size_t start_timestep, std::size_t num_timesteps, int iteration_number, double cost, const Eigen::MatrixXd ¶meters) |
Called by STOMP at the end of each iteration. More... | |
Protected Attributes | |
moveit::core::RobotStatePtr | goal_state_ |
std::string | group_name_ |
bool | has_goal_constraints_ |
bool | lock_goal_ |
bool | lock_start_ |
moveit::core::RobotModelConstPtr | robot_model_ |
moveit::core::RobotStatePtr | start_state_ |
True if a joint constraint for the goal was provided. | |
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.
|
overridevirtual |
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 152 of file joint_limits.cpp.