How to take into account max speed joint in motion planning ?

[sancelot]

How to take into account max speed joint in motion planning ?

[zkingston]

Seems to be a linking issue, can you check your CMakeLists.txt or the output build commands and verify that OMPL is being linked to your script?

[sancelot]

I found the pb , this is a call to parent class in overloaded function (the function does not exist in parent class...):
return ob::MotionValidator::checkMotion(s1, s2, lastValid);
The following piece of code works, however, I don't understand how to avoid computing unuseful trajectory for joints that are already at goal position (joints where start pos = goal pos...) ???

here is the output of the program :
Debug: RRT: Planner range detected to be 2.809924
Info: RRT: Starting planning with 1 states already in datastructure
Info: RRT: Created 34 states
Info: Solution found in 0.004124 seconds
Estimated time to execute trajectory: 3 seconds
Computed path with 4 waypoints:
Waypoint 0: 0 20.0003 0 0 0 0
Waypoint 1: -19.9215 84.0495 120.679 0 -16.2391 23.4193
Waypoint 2: -6.78866 61.6035 278.449 0 -5.53381 7.98061
Waypoint 3: 0 50.0007 360.005 0 0 0

#include <ompl/base/ScopedState.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/base/MotionValidator.h>

#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRT.h>

namespace ob = ompl::base;
namespace og = ompl::geometric;
double speedPercentage = 100;
#define radians(x) (x*3.14159/180.)
/*
If you know the maximum speed of all the robot joints and would like to compute a Cartesian linear trajectory using a general speed percentage, 
you can use an inverse kinematics solver to compute the joint angles corresponding to the start and goal positions of the end-effector. 
Once you have computed the start and goal joint angles, you can use a motion planning library such as OMPL to compute a linear trajectory in joint space that satisfies the maximum speed constraints for each joint.
*/
double maxSpeeds[6] = {radians(160), radians(120), radians(120),radians(225), radians(225), radians(225)}; // Maximum speed for each joint deg/sec
 

/*
This custom MotionValidator takes as input an array of maximum speeds for each joint and a desired speed percentage. 
It overrides the checkMotion method to reject motions where the distance between two consecutive states is greater than maxSpeeds[i] * speedPercentage for any joint i.*/
class MyMotionValidator : public ob::MotionValidator
{
  public:
    explicit MyMotionValidator(const ob::SpaceInformationPtr &si, double maxSpeeds[], double speedPercentage)
        : ob::MotionValidator(si), maxSpeeds_(maxSpeeds), speedPercentage_(speedPercentage)
    {
    }

    //virtual bool checkMotion(const ob::State* s1, const ob::State* s2) const
    virtual bool checkMotion(const ob::State *s1, const ob::State *s2) const override
    {
        // Check if the distance between the two states is greater than the maximum allowed distance for each joint
        const double *values1 = s1->as<ob::RealVectorStateSpace::StateType>()->values;
        const double *values2 = s2->as<ob::RealVectorStateSpace::StateType>()->values;
        for (int i = 0; i < 6; ++i)
        {
            if (std::abs(values2[i] - values1[i]) > maxSpeeds_[i] * speedPercentage_)
                return false;
        }


        // Use the default motion validation logic
        // return ob::MotionValidator::checkMotion(s1, s2);
        return true;
    }
 
    virtual bool checkMotion(const ompl::base::State *s1, const ompl::base::State *s2, std::pair<ompl::base::State *, double> &lastValid) const override
    {
        
        // Check if the distance between the two states is greater than the maximum allowed distance for each joint
        const double *values1 = s1->as<ob::RealVectorStateSpace::StateType>()->values;
        const double *values2 = s2->as<ob::RealVectorStateSpace::StateType>()->values;
        for (int i = 0; i < 6; ++i)
        {
            if (std::abs(values2[i] - values1[i]) > maxSpeeds_[i] * speedPercentage_)
                return false;
        }
        return true;
        // Use the default motion validation logic
       // return ob::MotionValidator::checkMotion(s1, s2, lastValid);
    }

  private:
    double *maxSpeeds_;
    double speedPercentage_;
};

// Define the robot's state validity checker
// a state is considered valid if it satisfies certain constraints, such as being collision-free or within the joint limits of the robot.
bool isStateValid(const ob::State *state)
{
    // Implement your state validity checking logic here
    // This function should return true if the state is valid, and false otherwise
    return true;
}

/*
The plan function is a custom function that can be used to compute a linear trajectory in joint space 
for a robot using the OMPL motion planning library. The function takes as input the start and goal joint angles, an array of maximum speeds for each joint, a desired speed percentage, and an output trajectory.*/

// Compute a linear trajectory with a given speed percentage
void plan(double start[], double goal[], double maxSpeed, double speedPercentage)
{
    // Create a state space (for a 3D robot with 6 joints)
    ob::StateSpacePtr space(new ob::RealVectorStateSpace(6));

    // Set bounds for each joint
    ob::RealVectorBounds bounds(6);
    bounds.setLow(-3.14159); // Example lower bound for joint angles
    bounds.setHigh(3.14159); // Example upper bound for joint angles
    bounds.setLow(3, -0.); // Lower bound for joint 2
    bounds.setHigh(3, 0.); // Upper bound for joint 2
    space->as<ob::RealVectorStateSpace>()->setBounds(bounds);

    // Create a simple setup
    og::SimpleSetup ss(space);

    // Set the state validity checker
    ss.setStateValidityChecker(isStateValid);

    // Create a start state
    ob::ScopedState<> startState(space);
    for (int i = 0; i < 6; ++i)
        startState[i] = start[i];

    // Create a goal state
    ob::ScopedState<> goalState(space);
    for (int i = 0; i < 6; ++i)
        goalState[i] = goal[i];

    // Set the start and goal states
    ss.setStartAndGoalStates(startState, goalState);

    // Specify the maximum length of a motion to be added to the tree
    //ss.getSpaceInformation()->getMaximumExtent(l);

    ss.getSpaceInformation()->setMotionValidator(ob::MotionValidatorPtr(new MyMotionValidator(ss.getSpaceInformation(), maxSpeeds, speedPercentage)));

    // Set the planner
    ob::PlannerPtr planner(new og::RRT(ss.getSpaceInformation()));
    ss.setPlanner(planner);

    // Set the planner termination condition
    ob::PlannerTerminationCondition ptc = ob::timedPlannerTerminationCondition(1.0); // Plan for 1 second

    // Attempt to solve the problem within the time limit
    ompl::base::PlannerStatus status = ss.solve(ptc);
    

    // Get the computed path
    og::PathGeometric path = ss.getSolutionPath();


// Compute the time needed to execute the trajectory
    double time = 0;
    for (std::size_t i = 1; i < path.getStateCount(); ++i)
    {
        const ob::State *state1 = path.getState(i - 1);
        const ob::State *state2 = path.getState(i);
        const double *values1 = state1->as<ob::RealVectorStateSpace::StateType>()->values;
        const double *values2 = state2->as<ob::RealVectorStateSpace::StateType>()->values;
        double maxTime = 0;
        for (int j = 0; j < 6; ++j)
        {
            double distance = std::abs(values2[j] - values1[j]);
            double speed = maxSpeeds[j] * speedPercentage / 100.0;
            double t = distance / speed;
            if (t > maxTime)
                maxTime = t;
        }
        time += maxTime;
    }

    std::cout << "Estimated time to execute trajectory: " << time << " seconds" << std::endl;
    // Convert the path to a trajectory
    /* 
 trajectory.clear();
 for (std::size_t i = 0; i < path.getStateCount(); ++i)
 {
 const ob::State* state = path.getState(i);
 const double* values = state->as<ob::RealVectorStateSpace::StateType>()->values;
 trajectory.push_back(std::vector<double>(values, values + 6));
 }
  */

    // Print the waypoints along the path
    std::cout << "Computed path with " << path.getStateCount() << " waypoints:" << std::endl;
    for (std::size_t i = 0; i < path.getStateCount(); ++i)
    {
        const ob::State *state = path.getState(i);
        const double *values = state->as<ob::RealVectorStateSpace::StateType>()->values;
        std::cout << "Waypoint " << i << ": ";
        for (int j = 0; j < 6; ++j)
            std::cout << values[j] * 180. / 3.141549<< " ";
        std::cout << std::endl;
    }
}
#

int main()
{
    // joints 
    double start[] = {0., radians(20.), 0, 0, 0, 0};
    double goal[] = {0, radians(50.),radians(360), 0, 0, 0};

    plan(start, goal, 100, 100);

}

[zkingston]

The algorithms in OMPL are sampling-based and thus will sometimes include spurious motion. I would use the simplification routines available (e.g., call ss.simplifySolution()) before extracting the solution trajectory and seeing if a better path can be found with local search.