Menu
▾
▴
Re: [ompl-users] no solution using RRT for 6dof robot arm
|
From: Mark M. <mm...@ri...> - 2021-04-08 17:30:50
|
Hi Trifena,
Your ValidityChecker::isValid function will be called a lot by OMPL’s planners. It looks like this method is doing a lot of initialization work that could be moved to the constructor. I suspect that this method is currently just really slow. If you call solve() with a much larger time threshold, you should see that it was able to generate more than just 1 state. If not, then there’s something else going wrong.
Best,
Mark
> On Apr 8, 2021, at 12:35 AM, Trifena Damaris <tri...@gm...> wrote:
>
>
> Dear OMPL developper,
>
> Hi. I just started using OMPL a couple days ago, so I am still trying to understand it.
> I am trying to use OMPL RRTConnect planner. The robot that i use is a 6-DOF puma arm robot.
> I consider a 6DOF joint space and I used the real vector space with bounds,
> and in the state validity checker I check the collision for the chosen path.
>
> I set the start point, goal point in joint angel for every joint(in radians) -> I'm not sure whether i use this correctly.
> When I try to run the code, it says that no solution is found. Is it because I declare the start and goal point wrongly?
>
> I am attaching the result and the code.
>
> Any help is appreciated! Thank you!
>
> Here is the code, I modified it from Optimal Planning Tutorial:
>
> #include <ompl/base/SpaceInformation.h>
> #include <ompl/base/spaces/SE3StateSpace.h>
> #include <ompl/geometric/planners/rrt/RRTConnect.h>
> #include <ompl/geometric/SimpleSetup.h>
> #include <ompl/geometric/planners/rrt/RRT.h>
> #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
> #include <ompl/base/Planner.h>
> #include <ompl/base/ProblemDefinition.h>
> #include <ompl/base/PlannerStatus.h>
>
> #include <ompl/config.h>
> #include <iostream>
> #include <cmath>
> #include <fstream>
>
> namespace ob = ompl::base;
> namespace og = ompl::geometric;
>
> class ValidityChecker : public ob::StateValidityChecker
> {
> public:
> ValidityChecker(const ob::SpaceInformationPtr &si) : ob::StateValidityChecker(si) {}
>
> bool isValid(const ob::State *state) const
> {
> const ob::RealVectorStateSpace::StateType *state_vec = state->as<ob::RealVectorStateSpace::StateType>();
>
> RobotCollisionDetector robot_collision_detector;
> Vector6d joint_6d;
> DepthImageGenerator depth_image_generator;
> MapGenerator map_generator;
> bool collision;
>
> PointCloudXYZRGB::Ptr obstacle_point_cloud;
> obstacle_point_cloud.reset(new PointCloudXYZRGB);
>
> //read image files
> //home
> cv::Mat rgb_image_home_main = cv::imread("0211_103231_572_main.png");
> cv::Mat rgb_image_home_sub = cv::imread("0211_103231_701_sub.png");
>
> //generate depth image
> cv::Mat depth_image_home = depth_image_generator.getDepthImage(rgb_image_home_main, rgb_image_home_sub);
>
> Pose pose_home;
> utility::setPose(&pose_home, 0.23573, -0.294639, 0.229634, 1.83002, -0.0000699457, 0.0000792287);
>
> map_generator.updateRGBMap(rgb_image_home_main, depth_image_home, pose_home);
>
> map_generator.getRGBObstacleMap(obstacle_point_cloud);
>
> for (int i = 0; i < 6; ++i)
> {
> std::cout << "joint:" << state_vec->values[i] << endl;
> joint_6d[i] = state_vec->values[i];
> }
>
> robot_collision_detector.checkCollision(joint_6d, obstacle_point_cloud, &collision);
> if (collision == true)
> {
> std::cout << "collision detected" << std::endl;
> return false;
> }
>
> return true;
> }
> };
>
> ob::OptimizationObjectivePtr getPathLengthObjective(const ob::SpaceInformationPtr &si)
> {
>
> ob::OptimizationObjectivePtr obj(new ob::PathLengthOptimizationObjective(si));
> obj->setCostThreshold(ob::Cost(M_PI));
> return obj;
> }
>
> void armPlanWithRRT()
> {
> // Construct the state space where we are planning
> ob::StateSpacePtr space(new ob::RealVectorStateSpace(6));
>
> ob::RealVectorBounds bounds(6);
> // for (int i = 0; i < 6; ++i)
> // {
> // bounds.setLow(i, -M_PI);
> // bounds.setHigh(i, M_PI);
> // }
>
> bounds.setLow(0, -3.05); bounds.setHigh(0, 3.05);
> bounds.setLow(1, -3.05); bounds.setHigh(1, 3.05);
> bounds.setLow(2, -2.79); bounds.setHigh(2, 2.79);
> bounds.setLow(3, -3.05); bounds.setHigh(3, 3.05);
> bounds.setLow(4, -2.40); bounds.setHigh(4, 2.40);
> bounds.setLow(5, -3.05); bounds.setHigh(5, 3.05);
>
>
> space->as<ob::RealVectorStateSpace>()->setBounds(bounds);
>
> // Construct a space information instance for this state space
> ob::SpaceInformationPtr si(new ob::SpaceInformation(space));
>
> // Set the object used to check which states in the space are valid
> si->setStateValidityChecker(ob::StateValidityCheckerPtr(new ValidityChecker(si)));
> si->setup();
>
> // Setup Start and Goal
> ob::ScopedState<ob::RealVectorStateSpace> start(space);
> start->as<ob::RealVectorStateSpace::StateType>()->values[0] = -0.777708;
> start->as<ob::RealVectorStateSpace::StateType>()->values[1] = -0.26842;
> start->as<ob::RealVectorStateSpace::StateType>()->values[2] = -1.80781;
> start->as<ob::RealVectorStateSpace::StateType>()->values[3] = 2.02778;
> start->as<ob::RealVectorStateSpace::StateType>()->values[4] = 2.24392;
> start->as<ob::RealVectorStateSpace::StateType>()->values[5] = 2.2386;
> start.print(std::cout);
>
> ob::ScopedState<ob::RealVectorStateSpace> goal(space);
> goal->as<ob::RealVectorStateSpace::StateType>()->values[0] = -2.01023;
> goal->as<ob::RealVectorStateSpace::StateType>()->values[1] = -0.31717;
> goal->as<ob::RealVectorStateSpace::StateType>()->values[2] = -1.62667;
> goal->as<ob::RealVectorStateSpace::StateType>()->values[3] = 1.40112;
> goal->as<ob::RealVectorStateSpace::StateType>()->values[4] = 1.16346;
> goal->as<ob::RealVectorStateSpace::StateType>()->values[5] = 1.97896;
> goal.print(std::cout);
>
> // Create a problem instance
> ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
>
> // Set the start and goal states
> pdef->setStartAndGoalStates(start, goal);
>
> pdef->setOptimizationObjective(getPathLengthObjective(si));
>
> // Construct our optimizing planner using the RRTstar algorithm.
> ob::PlannerPtr optimizingPlanner(new ompl::geometric::RRTConnect(si));
>
> // Set the problem instance for our planner to solve
> optimizingPlanner->setProblemDefinition(pdef);
> optimizingPlanner->setup();
>
> ob::PlannerStatus solved = optimizingPlanner->solve(1.0);
>
> if (solved)
> {
>
> std::cout << "Found solution:" << std::endl;
> pdef->getSolutionPath()->print(std::cout);
> }
> else
> {
> std::cout << "No solution found" << std::endl;
> }
>
> optimizingPlanner->clear();
> }
>
> int main(int /*argc*/, char ** /*argv*/)
> {
>
> std::cout << "*********************planRRT*****************" << std::endl
> << std::endl;
>
> armPlanWithRRT();
>
> return 0;
> }
>
>
>
> The result when i tried to run the code:
> *********************planRRT*****************
>
> RealVectorState [-0.777708 -0.26842 -1.80781 2.02778 2.24392 2.2386]
> RealVectorState [-2.01023 -0.31717 -1.62667 1.40112 1.16346 1.97896]
> Debug: RRTConnect: Planner range detected to be 2.849676
> [MAP GENERATOR] obstacle_cloud_ size_after_update: 7987
>
> joint:-0.777708
> joint:-0.26842
> joint:-1.80781
> joint:2.02778
> joint:2.24392
> joint:2.2386
>
> no collision detected
> Info: RRTConnect: Starting planning with 1 states already in datastructure
> Info: RRTConnect: Created 1 states (1 start + 0 goal)
> No solution found
>
>
>
>
> _______________________________________________
> ompl-users mailing list
> omp...@li...
> https://lists.sourceforge.net/lists/listinfo/ompl-users
|
