OMPL planner for a single joint

[Spyros_G]

Hello everyone,

I am using OMPL to plan the motion for a single joint (like a pendulum) and I can not find out how exactly the motion is planned. I have found no information on how the waypoints are being created. I am asking this because my initial purpose was to apply a trapezoid velocity profile. All I have found is this page (http://ompl.kavrakilab.org/planners.html) but the planners listed seem to deal more with navigating through an environment with obstacles (for mobile robots for example). Thank you all for your time.

Spyros G

[Simon Schmeißer]

Hello Spyros,

for a single joint no planning can be done as (most of the planners) don't care about time/momentum etc. So planning along a single joint would only give you a yes/no answer depending on whether there are collisions on the way or not. Nothing more

I think you are confusing terms here. Do you want to simulate (physically?) your joint? Search for Gazebo or similar. Do you want to parametrize the movement along this axis? Google for trajectory_processing::IterativeParabolicTimeParameterization. Do you want to do collision checking? Create a robot model (URDF), set the joint position in discrete steps and use PlanningScene::checkCollision (or similar)

Simon

[Spyros_G]

Hello Simon,

thanks for your reply.

I am already simulating the joint in Gazebo and I am using MoveIt! (and OMPL) to plan the trajectory.

When I say trajectory planning I mean the designing of the desired position through time - desired_angle(t). I think this is what MoveIt! is developing in the form of waypoints, am I mistaken?

In any case, my original question could be rephrased as how does MoveIt! generating those trajectories for each joint.

Thanks again for your time. I am sorry if I am asking trivial questions but it is the first time I am dealing with this software.

Spyros

[Simon Schmeißer]

Hello Spyros,

OMPL will only give you desired_angle(i) but not desired_angle(t), where i is an index of points on the trajectory. However for a single rotational axis there is only one ( or two ) ways to get from angle1 to angle2, so OMPL is not needed here. you can just simply interpolate between two RobotState with a stepsize that depends on how precise you need to check collisions. Then check collisions for each angle inbetween. If there are none you can execute your trajectory by whatever means suite you.

To go from desired_angle(i) to desired_angle(t) you need to do time parametrization. You can check out trajectory_processing::IterativeParabolicTimeParameterization for an example. But probably there are simpler solutions for single axis systems.

You're welcome


Here's a piece of sample code, you will need to adapt it a bit to suite your needs:


    robot_state::RobotState intermediateState(state);

    bool collisionFree = true;

    {  // Scope LockedPlanningSceneRO
        planning_scene_monitor::LockedPlanningSceneRO ps(m_rosWorld->getPlanningSceneMonitor());
        int steps = 100; //Todo: make this dependent on distance

        for (int i = 0; i <= steps; i++) {
            state.interpolate(goalState, i*1.0/steps, intermediateState);
            intermediateState.update();
            intermediateState.updateCollisionBodyTransforms();
            if (!ps->isStateValid(intermediateState, "manipulator")) {
                ROS_ERROR("Collision in direct moving at %f, will start planner", i*1.0/steps);

                collision_detection::CollisionResult::ContactMap contacts;
                ps->getCollidingPairs(contacts, intermediateState);

                for (collision_detection::CollisionResult::ContactMap::const_iterator it = contacts.begin(); it != contacts.end(); ++it) {
                    std::cout << "Collision between " << it->first.first << " and " << it->first.second << std::endl;
                }

                collisionFree = false;
                break;
            }
        }
    }  // Scope LockedPlanningSceneRO

    bool pathFound = collisionFree;

    if (collisionFree) {
        std::cout << "Path is clear, moving directly" << std::endl;
        robot_trajectory::RobotTrajectory traj(state.getRobotModel(), "manipulator");

        double nulls [6]= {0, 0, 0, 0, 0, 0};
        goalState.setVariableVelocities(nulls);
        goalState.setVariableAccelerations(nulls);

        robot_state::RobotState wSpeed(state);
        wSpeed.setVariableVelocities(nulls);
        wSpeed.setVariableAccelerations(nulls);
        traj.addSuffixWayPoint(wSpeed, 1e-4);
        traj.addSuffixWayPoint(goalState, 0.01); //Todo: calculate time?
        trajectory_processing::IterativeParabolicTimeParameterization timeGuess;
        bool success = timeGuess.computeTimeStamps(traj);

        traj.getLastWayPointPtr()->setVariableAccelerations(nulls);

        std::cout << "Time parametrization: " << (success?"yes":"no") << traj.getWayPointDurations().back() << std::endl;
        std::cout << "num waypoints: " << traj.getWayPointCount() << std::endl;

        moveit::core::robotStateToRobotStateMsg(wSpeed, plan.start_state_);
        traj.getRobotTrajectoryMsg(plan.trajectory_);