speed control in Hybrid Planning

[YJChen2021]

Hi all, I am currently trying to dynamically control the velocity of my robot arm with Hybrid Planning in Gazebo simulation, particularly to slow down the robot's velocity.

I have successfully controlled my robot arm with hybrid planning and have also been able to implement my custom plugin. My objective is to implement a custom local_constraint_solver plugin, which should allow me to control the velocity of my robot. Therefore, I am now looking for an exact method to control the robot's speed. My initial approach was to modify the duration of robot_command.addSuffixWayPoint(); at line 99 and line 124 in the plugin from the Moveit2 Hybrid Planning Plugin.

  if (!stop_before_collision_)
  {
    robot_command.addSuffixWayPoint(local_trajectory.getWayPoint(0), 0.0);// line 99
  }

if (is_path_valid)
{
  if (path_invalidation_event_send_)
  {
    path_invalidation_event_send_ = false;  // Reset flag
  }
  // Forward next waypoint to the robot controller
  robot_command.addSuffixWayPoint(local_trajectory.getWayPoint(0), 0.0); // line 124
}

Although this did lower the robot's velocity, the robot's movement was not as smooth as expected. The robot often shook and failed to reach the goal. By checking into the log, I suspect the reason might be the process stuck between lines 156 and 168. Are there any suggestions for controlling a robot's speed when using Hybrid Planning or any possible reasons for the jerky motion or failure to reach the goal?

if (prev_waypoint_target_->distance(*robot_command.getFirstWayPointPtr()) <= STUCK_THRESHOLD_RAD) // line 156
{
  ++num_iterations_stuck_;
  if (num_iterations_stuck_ > STUCK_ITERATIONS_THRESHOLD)
  {
    num_iterations_stuck_ = 0;
    prev_waypoint_target_ = nullptr;
    feedback_result.feedback = toString(LocalFeedbackEnum::LOCAL_PLANNER_STUCK);
    path_invalidation_event_send_ = true;  // Set feedback flag
    RCLCPP_INFO(node_->get_logger(), "The local planner has been stuck for several iterations. Aborting.");
  }
}
prev_waypoint_target_ = robot_command.getFirstWayPointPtr();// line 168

This is my first time posing a question here, I would greatly appreciate any advice or suggestions. Thank you for any help.

[Mat198]

Hello, I'm doing something similar on my robot. My approach was to set the new velocity in the current waypoint that I send to the hardware interface. You can do changing the line 107.

RobotState targetState = localTrajectory.getWayPoint(0);
std::vector<double> velocity(6, 1.0);
targetState.setVariableVelocities(velocity);
obotTrajectory.addSuffixWayPoint(targetState, 0.0);

It should work as long as your robot is configured in velocity mode. You can check that using:

if (targetState.hasVelocities()) {
    // Update Velocity
}
if (targetState.hasAccelerations()) {
    // Update Acceleration
}

[Mat198]

Please don't post your code as image. Post the code as text so people can google it. And the best place to ask such question in on the https://robotics.stackexchange.com/

[YJChen2021]

@Mat198 Hello,

Thank you very much for your help in advance. Your approach seems reasonable, and I will try it when I have some free time.

Regarding posting the code as an image, I would prefer to post my code as text. However, I couldn’t find a way to include line numbers, which makes it difficult to explain without them. I will try to modify it accordingly. Thank you very much for your assistance

[gaspatxo]

Hi @YJChen2021. I like posting the entire code at the end of your post. To reference specific lines, I find it best to write the lines you reference.

Here is an example:

Hi, can someone explain what the symbol := does in this line?

    if name := entry.get("name"):

Here is the whole code for context:

sample_data = [
    {"userId": 1,  "name": "rahul", "completed": False},
    {"userId": 1, "name": "rohit", "completed": False},
    {"userId": 1,  "name": "ram", "completed": False},
    {"userId": 1,  "name": "ravan", "completed": True}
]

print("With Python 3.8 Walrus Operator:") 
for entry in sample_data: 
    if name := entry.get("name"):
        print(f'Found name: "{name}"')

---

Readability counts, the better you format your code and posts, the more willing and able are other people to help you. Keep up the great work.

[YJChen2021]

Hi @YJChen2021. I like posting the entire code at the end of your post. To reference specific lines, I find it best to write the lines you reference.

Here is an example:

Hi, can someone explain what the symbol := does in this line?

    if name := entry.get("name"):

Here is the whole code for context:

sample_data = [
    {"userId": 1,  "name": "rahul", "completed": False},
    {"userId": 1, "name": "rohit", "completed": False},
    {"userId": 1,  "name": "ram", "completed": False},
    {"userId": 1,  "name": "ravan", "completed": True}
]

print("With Python 3.8 Walrus Operator:") 
for entry in sample_data: 
    if name := entry.get("name"):
        print(f'Found name: "{name}"')

Readability counts, the better you format your code and posts, the more willing and able are other people to help you. Keep up the great work.

@gaspatxo Thank you for your example, I have updated my issue with the exact code instead of posting images.