Merge pull request ros-industrial#21 from marip8/fix/goal_ik_solve

Goal State IK Solve Bug Fix
photoneo · May 5, 2020 · 7fe40fb · 7fe40fb
2 parents 2377bc2 + 64cdd98
commit 7fe40fb
Showing 1 changed file with 2 additions and 3 deletions.
diff --git a/stomp_moveit/src/stomp_planner.cpp b/stomp_moveit/src/stomp_planner.cpp
@@ -409,9 +409,8 @@ bool StompPlanner::getSeedParameters(Eigen::MatrixXd& parameters) const
     }
 
     Eigen::VectorXd solution;
-    Eigen::VectorXd seed = start;
     ik_solver_->setKinematicState(state);
-    if(ik_solver_->solve(seed,tool_constraints.get(),solution))
+    if(ik_solver_->solve(goal,tool_constraints.get(),solution))
     {
       goal = solution;
       found_goal = true;
@@ -722,7 +721,7 @@ bool StompPlanner::canServiceRequest(const moveit_msgs::MotionPlanRequest &req)
   // check that we have joint or cartesian constraints at the goal
   const auto& gc = req.goal_constraints[0];
   if ((gc.joint_constraints.size() == 0) &&
-		  !utils::kinematics::isCartesianConstraints(gc))
+      !utils::kinematics::isCartesianConstraints(gc))
   {
     ROS_ERROR("STOMP couldn't find either a joint or cartesian goal.");
     return false;