Skip to content

Commit

Permalink
PR changes
Browse files Browse the repository at this point in the history 
PR changes
  • Loading branch information
@MichalDobis
MichalDobis committed Jul 8, 2021
1 parent aa07491 commit a46835b
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 89 deletions.
21 changes: 3 additions & 18 deletions stomp_plugins/include/stomp_plugins/noise_generators/cartesian_constraints_sampling.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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<double, 6> ik_tolerance_ = {0.001, 0.001, 0.001, 0.01, 0.01, 0.01};

// ros parameters
std::vector<double> stddev_; /**< @brief The standard deviations applied to each cartesian DOF **/

// noisy trajectory generation
Eigen::VectorXd raw_noise_; /**< @brief The noise vector **/
std::array<double, 6> stddev_; /**< @brief The standard deviations applied to each cartesian DOF **/

// random goal generation
boost::shared_ptr<RandomGenerator> goal_rand_generator_; /**< @brief Random generator for the tool pose **/
Expand Down
91 changes: 20 additions & 71 deletions stomp_plugins/src/noise_generators/cartesian_constraints_sampling.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,12 +31,7 @@

PLUGINLIB_EXPORT_CLASS(stomp_moveit::noise_generators::CartesianConstraintsSampling,stomp_moveit::noise_generators::StompNoiseGenerator);

static const std::vector<double> 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<int> 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
{
Expand All @@ -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)
{
Expand All @@ -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);
}

Expand All @@ -96,6 +85,14 @@ bool CartesianConstraintsSampling::configure(const XmlRpc::XmlRpcValue& config)

// noise generation parameters
XmlRpcValue stddev_param = params["stddev"];
tool_link_ = static_cast<std::string>(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())
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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<const VectorXd>(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);
Expand All @@ -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());
}

Expand Down

0 comments on commit a46835b

Please sign in to comment.