diff --git a/stomp_plugins/include/stomp_plugins/noise_generators/cartesian_constraints_sampling.h b/stomp_plugins/include/stomp_plugins/noise_generators/cartesian_constraints_sampling.h index 28e6367..263d0fc 100644 --- a/stomp_plugins/include/stomp_plugins/noise_generators/cartesian_constraints_sampling.h +++ b/stomp_plugins/include/stomp_plugins/noise_generators/cartesian_constraints_sampling.h @@ -15,10 +15,7 @@ typedef boost::variate_generator< RGNType, boost::uniform_real<> > RandomGenerat /** * @class stomp_moveit::noise_generators::CartesianConstraintsSampling - * @brief This class generates noisy trajectories to an under-constrained cartesian goal pose. - * - * @par Examples: - * All examples are located here @ref stomp_plugins_examples + * @brief This class generates noisy trajectories to an under-constrained cartesian waypoints. * */ class CartesianConstraintsSampling: public StompNoiseGenerator @@ -100,15 +97,6 @@ class CartesianConstraintsSampling: public StompNoiseGenerator protected: - virtual bool setupNoiseGeneration(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::MotionPlanRequest &req, - const stomp_core::StompConfiguration &config, - moveit_msgs::MoveItErrorCodes& error_code); - - virtual bool setupRobotState(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::MotionPlanRequest &req, - const stomp_core::StompConfiguration &config, - moveit_msgs::MoveItErrorCodes& error_code); /** * @brief Genereates a random tool pose by apply noise on the redundant axis to a reference tool pose; * @param reference_joint_pose Joint position used in computing the reference tool pose with FK @@ -125,13 +113,10 @@ class CartesianConstraintsSampling: public StompNoiseGenerator // tool link and ik tolerance std::string tool_link_; - Eigen::VectorXd tool_goal_tolerance_; + const std::array ik_tolerance_ = {0.001, 0.001, 0.001, 0.01, 0.01, 0.01}; // ros parameters - std::vector stddev_; /**< @brief The standard deviations applied to each cartesian DOF **/ - - // noisy trajectory generation - Eigen::VectorXd raw_noise_; /**< @brief The noise vector **/ + std::array stddev_; /**< @brief The standard deviations applied to each cartesian DOF **/ // random goal generation boost::shared_ptr goal_rand_generator_; /**< @brief Random generator for the tool pose **/ diff --git a/stomp_plugins/src/noise_generators/cartesian_constraints_sampling.cpp b/stomp_plugins/src/noise_generators/cartesian_constraints_sampling.cpp index afa361d..8e1727a 100644 --- a/stomp_plugins/src/noise_generators/cartesian_constraints_sampling.cpp +++ b/stomp_plugins/src/noise_generators/cartesian_constraints_sampling.cpp @@ -31,12 +31,7 @@ PLUGINLIB_EXPORT_CLASS(stomp_moveit::noise_generators::CartesianConstraintsSampling,stomp_moveit::noise_generators::StompNoiseGenerator); -static const std::vector ACC_MATRIX_DIAGONAL_VALUES = {-1.0/12.0, 16.0/12.0, -30.0/12.0, 16.0/12.0, -1.0/12.0}; -static const std::vector ACC_MATRIX_DIAGONAL_INDICES = {-2, -1, 0 ,1, 2}; static const int CARTESIAN_DOF_SIZE = 6; -static const double DEFAULT_IK_POS_TOLERANCE = 0.001; -static const double DEFAULT_IK_ROT_TOLERANCE = 0.01; - namespace stomp_moveit { @@ -63,6 +58,10 @@ bool CartesianConstraintsSampling::initialize(moveit::core::RobotModelConstPtr r // robot model details group_ = group_name; robot_model_ = robot_model_ptr; + + // robot state + state_.reset(new RobotState(robot_model_)); + const JointModelGroup* joint_group = robot_model_ptr->getJointModelGroup(group_name); if(!joint_group) { @@ -73,16 +72,6 @@ bool CartesianConstraintsSampling::initialize(moveit::core::RobotModelConstPtr r // kinematics ik_solver_.reset(new stomp_moveit::utils::kinematics::IKSolver(robot_model_ptr,group_name)); - // trajectory noise generation - stddev_.resize(CARTESIAN_DOF_SIZE); - - // creating default cartesian tolerance for IK solver - tool_goal_tolerance_.resize(CARTESIAN_DOF_SIZE); - double ptol = DEFAULT_IK_POS_TOLERANCE; - double rtol = DEFAULT_IK_ROT_TOLERANCE; - tool_goal_tolerance_ << ptol, ptol, ptol, rtol, rtol, rtol; - - return configure(config); } @@ -96,6 +85,14 @@ bool CartesianConstraintsSampling::configure(const XmlRpc::XmlRpcValue& config) // noise generation parameters XmlRpcValue stddev_param = params["stddev"]; + tool_link_ = static_cast(params["link_id"]); + + // Check link_id + if (tool_link_.empty()) + { + ROS_ERROR("%s the 'link_id' parameter is empty",getName().c_str()); + return false; + } // check stddev if(stddev_param.size() != stddev_.size()) @@ -123,62 +120,11 @@ bool CartesianConstraintsSampling::setMotionPlanRequest(const planning_scene::Pl const moveit_msgs::MotionPlanRequest &req, const stomp_core::StompConfiguration &config, moveit_msgs::MoveItErrorCodes& error_code) -{ - bool succeed = setupNoiseGeneration(planning_scene,req,config,error_code) && - setupRobotState(planning_scene,req,config,error_code); - return succeed; -} - -bool CartesianConstraintsSampling::setupNoiseGeneration(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::MotionPlanRequest &req, - const stomp_core::StompConfiguration &config, - moveit_msgs::MoveItErrorCodes& error_code) -{ - using namespace Eigen; - - // convenience lambda function to fill matrix - auto fill_diagonal = [](Eigen::MatrixXd& m,double coeff,int diag_index) - { - std::size_t size = m.rows() - std::abs(diag_index); - m.diagonal(diag_index) = VectorXd::Constant(size,coeff); - }; - - // creating finite difference acceleration matrix - std::size_t num_timesteps = config.num_timesteps; - Eigen::MatrixXd A = MatrixXd::Zero(num_timesteps,num_timesteps); - int num_elements = (int((ACC_MATRIX_DIAGONAL_INDICES.size() -1)/2.0) + 1)* num_timesteps ; - for(auto i = 0u; i < ACC_MATRIX_DIAGONAL_INDICES.size() ; i++) - { - fill_diagonal(A,ACC_MATRIX_DIAGONAL_VALUES[i],ACC_MATRIX_DIAGONAL_INDICES[i]); - } - - // create and scale covariance matrix - Eigen::MatrixXd covariance = MatrixXd::Identity(num_timesteps,num_timesteps); - covariance = A.transpose() * A; - covariance = covariance.fullPivLu().inverse(); - double max_val = covariance.array().abs().matrix().maxCoeff(); - covariance /= max_val; - - // preallocating noise data - raw_noise_ = Eigen::VectorXd::Zero(CARTESIAN_DOF_SIZE); - - error_code.val = error_code.SUCCESS; - - return true; -} - -bool CartesianConstraintsSampling::setupRobotState(const planning_scene::PlanningSceneConstPtr& planning_scene, - const moveit_msgs::MotionPlanRequest &req, - const stomp_core::StompConfiguration &config, - moveit_msgs::MoveItErrorCodes& error_code) { using namespace moveit::core; using namespace utils::kinematics; - // robot state - const JointModelGroup* joint_group = robot_model_->getJointModelGroup(group_); - tool_link_ = joint_group->getLinkModelNames().back(); - state_.reset(new RobotState(robot_model_)); + // Update robot state robotStateMsgToRobotState(req.start_state,*state_); // update kinematic model @@ -209,7 +155,9 @@ bool CartesianConstraintsSampling::generateNoise(const Eigen::MatrixXd& paramete { const auto noisy_tool_pose = applyCartesianNoise(parameters.col(t)); Eigen::VectorXd result = Eigen::VectorXd::Zero(parameters.rows()); - if(!ik_solver_->solve(parameters.col(t),noisy_tool_pose,result,tool_goal_tolerance_)) + + const Eigen::VectorXd ik_tolerance_eigen = Map(ik_tolerance_.data(), CARTESIAN_DOF_SIZE); + if(!ik_solver_->solve(parameters.col(t),noisy_tool_pose,result,ik_tolerance_eigen)) { ROS_DEBUG("%s could not compute ik, returning noiseless goal pose",getName().c_str()); parameters_noise.col(t) = parameters.col(t); @@ -233,15 +181,16 @@ Eigen::Affine3d CartesianConstraintsSampling::applyCartesianNoise(const Eigen::V const JointModelGroup* joint_group = robot_model_->getJointModelGroup(group_); - for(auto d = 0u; d < raw_noise_.size(); d++) + Eigen::VectorXd raw_noise = Eigen::VectorXd::Zero(CARTESIAN_DOF_SIZE); + for(auto d = 0u; d < raw_noise.size(); d++) { - raw_noise_(d) = stddev_[d]*(*goal_rand_generator_)(); + raw_noise(d) = stddev_[d]*(*goal_rand_generator_)(); } state_->setJointGroupPositions(joint_group,reference_joint_pose); Eigen::Affine3d tool_pose = state_->getFrameTransform(tool_link_); - auto& n = raw_noise_; + const auto& n = raw_noise; return tool_pose * Translation3d(Vector3d(n(0),n(1),n(2)))*AngleAxisd(n(3),Vector3d::UnitX())*AngleAxisd(n(4),Vector3d::UnitY())*AngleAxisd(n(5),Vector3d::UnitZ()); }