So I'm trying to change the `solve_type` to "Distance" as opposed to "Speed" for TRAC IK. RRTConnect and TRAC IK acknowledge the change, but I'm getting an error in the console and a failed compute solve. (it doesn't fail with Speed, and the target position I'm trying is essentially the start position).
The error is as follows:
Error: RRTConnect: Unable to sample any valid states for goal tree
at line 215 in /tmp/binarydeb/ros-kinetic-ompl-1.2.1/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp
The code I'm using to solve is as follows:
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "world";
pose.pose.position.x = messageTarget.x();
pose.pose.position.y = messageTarget.y();
pose.pose.position.z = messageTarget.z();
pose.pose.orientation.w = 1;
std::vector tolerancePose(3, 0.01);
std::vector toleranceAngle(3, 0.01);
req.workspace_parameters.min_corner.x = -50;
req.workspace_parameters.min_corner.y = -50;
req.workspace_parameters.min_corner.z = -50;
req.workspace_parameters.max_corner.x = 50;
req.workspace_parameters.max_corner.y = 50;
req.workspace_parameters.max_corner.z = 50;
req.group_name = "arm";
req.start_state.joint_state = this->getCurrentJointState(true);
moveit_msgs::Constraints poseGoal =
kinematic_constraints::constructGoalConstraints("ee_link", pose, tolerancePose, toleranceAngle);
req.goal_constraints.push_back(poseGoal);
planning_interface::PlanningContextPtr context =
this->plannerInstance->getPlanningContext(this->planningScene, req, res.error_code_);
context->solve(res);
Again, it works with "Speed" but not with "Distance".
My `kinematics.yml` file looks like this:
arm:
solve_type: Distance
kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
kinematics_solver_timeout: 10
I wouldn't mind changing to a different planner (if I could figure out how), but I think TRAC IK is right for our application.
Is there a way I can fix it? Am I not including the right information in the request?
↧