diff --git a/src/reach_study.cpp b/src/reach_study.cpp index 638eeb5..bfdd13f 100644 --- a/src/reach_study.cpp +++ b/src/reach_study.cpp @@ -108,7 +108,7 @@ void ReachStudy::run() #pragma omp parallel for num_threads(params_.max_threads) for (std::size_t i = 0; i < target_poses_.size(); ++i) { - const Eigen::Isometry3d& tgt_frame = target_poses_[i] * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()); + const Eigen::Isometry3d& tgt_frame = target_poses_[i]; // Solve IK try