Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

CHOMP doesn't respect joint constraints. #3135

Open
aizmailovs opened this issue Nov 27, 2024 · 0 comments
Open

CHOMP doesn't respect joint constraints. #3135

aizmailovs opened this issue Nov 27, 2024 · 0 comments
Labels
bug Something isn't working

Comments

@aizmailovs
Copy link

Description

CHOMP does not respect the joint constraints (moveit_msgs::msg::JointConstraint) when planning a motion.

ROS Distro

Humble

OS and version

Ubuntu 22.04.

Source or binary build?

Binary

If binary, which release version?

2.5.5.

If source, which branch?

No response

Which RMW are you using?

None

Steps to Reproduce

  1. Get a working robot arm simulation in Moveit2, created using Moveit Setup Assistant. Doesn't matter which robot in particular is in use.
    If it is possible plan and execute motions using this simulation - the step succeeded.

  2. Install CHOMP motion planner sudo apt-get install ros-humble-moveit-planners-chomp.

  3. Create and add CHOMP config file chomp_planning.yaml at <robot>_moveit_config/config.
    File content in my case:

planning_plugin: chomp_interface/CHOMPPlanner
enable_failure_recovery: true
jiggle_fraction: 0.05
request_adapters: >-
    default_planner_request_adapters/AddTimeOptimalParameterization
    default_planner_request_adapters/ResolveConstraintFrames
    default_planner_request_adapters/FixWorkspaceBounds
    default_planner_request_adapters/FixStartStateBounds
    default_planner_request_adapters/FixStartStateCollision
    default_planner_request_adapters/FixStartStatePathConstraints
ridge_factor: 0.01
start_state_max_bounds_error: 0.1
trajectory_initialization_method: "linear"
  1. Add CHOMP planning pipeline:
    moveit_config = (
        MoveItConfigsBuilder("<robot>", package_name="<robot>_moveit_config")
        .planning_pipelines(pipelines=["ompl", "chomp"])
        .to_moveit_configs()
    )
  1. Create a node to:
  • apply joint constraints,
  • set planning goal (in joint-space),
  • set CHOMP planning pipeline,
  • plan a motion

Here is the code that I used for it:

#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char *argv[])
{
    // Initialize ROS and create the Node
    rclcpp::init(argc, argv);
    auto const node = std::make_shared<rclcpp::Node>(
        "joint_constraints_node",
        rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

    // Create a ROS logger
    auto const logger = rclcpp::get_logger("joint_constraints_node");

    // Spin up a SingleThreadedExecutor
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    auto spinner = std::thread([&executor]()
                                { executor.spin(); });

    const std::string planning_group_name = "<robot_planning_group>";

    // Create the MoveIt MoveGroup Interface
    using moveit::planning_interface::MoveGroupInterface;
    auto move_group_interface = MoveGroupInterface(node, planning_group_name);

    // Define joint constraints
    moveit_msgs::msg::Constraints mixed_constraints;
    moveit_msgs::msg::JointConstraint joint1_constraint;

    joint1_constraint.joint_name = "joint_1";
    joint1_constraint.position = 0.0;
    joint1_constraint.tolerance_below = 0.0872665;
    joint1_constraint.tolerance_above = 1.65806;
    joint1_constraint.weight = 0.2;

    mixed_constraints.joint_constraints.emplace_back(joint1_constraint);
    move_group_interface.setPathConstraints(mixed_constraints);

    move_group_interface.setPlanningTime(5.0);

    std::vector<double> joint_target_values = {1.573686, 0.834745, -0.424615, 3.203549, 0.312052, 1.508935};

    move_group_interface.setPlanningPipelineId("chomp");

    move_group_interface.setJointValueTarget(joint_target_values);

    auto const [success, plan] = [&move_group_interface, &logger]()
    {
        moveit::planning_interface::MoveGroupInterface::Plan msg;
        bool ok = false;

        try
        {
            ok = static_cast<bool>(move_group_interface.plan(msg));
        }
        catch (const std::exception& e)
        {
            RCLCPP_ERROR(logger, "Exception caught during path planning: %s", e.what());
            ok = false;
        }

        return std::make_pair(ok, msg);
    }();

    if (success)
    {
        RCLCPP_INFO(logger, "Path planning successful. Executing the plan...");
        move_group_interface.execute(plan);
    }
    else {
        RCLCPP_INFO(logger, "Path planning failed.");
    }

    // Shutdown ROS
    rclcpp::shutdown();
    spinner.join();
    return 0;
}

To make sure that joint constraints impact the motion planning, I advice to:

  1. Run a motion planning using CHOMP without constraints.
  2. Apply the constraints to make the initial motion invalid.
  3. Rerun the motion planning with the added constraints.

Expected behavior

CHOMP takes into account added joint constraints when planning a motion.

Actual behavior

CHOMP plans a "valid" motion, which gets rejected by Moveit due to joint constraints violation.

Backtrace or Console output

[move_group-3] [INFO] [1732712673.305495682] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-3] [INFO] [1732712673.305630717] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-3] [INFO] [1732712673.315543690] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-3] [INFO] [1732712673.315599084] [moveit_move_group_capabilities_base.move_group_capability]: Using planning pipeline 'chomp'
[move_group-3] [INFO] [1732712677.612108002] [chomp_planner]: CHOMP trajectory initialized using method: linear 
[move_group-3] [INFO] [1732712677.612151754] [chomp_optimizer]: Active collision detector is: HYBRID
[move_group-3] [INFO] [1732712677.734206054] [chomp_optimizer]: First coll check took 0.122031 sec
[move_group-3] [INFO] [1732712677.749310245] [chomp_optimizer]: Collision cost 66.587672, smoothness cost: 47.218001
[move_group-3] [INFO] [1732712677.749393102] [chomp_optimizer]: iteration: 0
[move_group-3] [INFO] [1732712677.749919245] [chomp_optimizer]: cCost 66.587672 over threshold 0.070000
[move_group-3] [INFO] [1732712677.758952307] [chomp_optimizer]: Collision cost 68.609067, smoothness cost: 47.050323
[move_group-3] [INFO] [1732712677.759092902] [chomp_optimizer]: cCost 68.609067 over threshold 0.070000
[move_group-3] [INFO] [1732712677.768513775] [chomp_optimizer]: Collision cost 86.200873, smoothness cost: 47.027408
[move_group-3] [INFO] [1732712677.768601581] [chomp_optimizer]: cCost 86.200873 over threshold 0.070000
[move_group-3] [INFO] [1732712677.777561364] [chomp_optimizer]: Collision cost 92.017880, smoothness cost: 46.945328
[move_group-3] [INFO] [1732712677.777637398] [chomp_optimizer]: cCost 92.017880 over threshold 0.070000
[move_group-3] [INFO] [1732712677.786377166] [chomp_optimizer]: Collision cost 91.856942, smoothness cost: 46.773505
[move_group-3] [INFO] [1732712677.786437049] [chomp_optimizer]: cCost 91.856942 over threshold 0.070000
[move_group-3] [INFO] [1732712677.795559259] [chomp_optimizer]: Collision cost 92.577747, smoothness cost: 46.613688
[move_group-3] [INFO] [1732712677.795670108] [chomp_optimizer]: cCost 92.577747 over threshold 0.070000
[move_group-3] [INFO] [1732712677.804653065] [chomp_optimizer]: Collision cost 91.270322, smoothness cost: 46.453027
[move_group-3] [INFO] [1732712677.804727194] [chomp_optimizer]: cCost 91.270322 over threshold 0.070000
[move_group-3] [INFO] [1732712677.813632716] [chomp_optimizer]: Collision cost 90.707015, smoothness cost: 46.294119
[move_group-3] [INFO] [1732712677.813689032] [chomp_optimizer]: cCost 90.707015 over threshold 0.070000
[move_group-3] [INFO] [1732712677.822435081] [chomp_optimizer]: Collision cost 93.814291, smoothness cost: 46.145039
[move_group-3] [INFO] [1732712677.822496698] [chomp_optimizer]: cCost 93.814291 over threshold 0.070000
[move_group-3] [INFO] [1732712677.831799979] [chomp_optimizer]: Collision cost 93.447526, smoothness cost: 45.987866
[move_group-3] [INFO] [1732712677.831883316] [chomp_optimizer]: cCost 93.447526 over threshold 0.070000
[move_group-3] [INFO] [1732712677.840731629] [chomp_optimizer]: Collision cost 95.457955, smoothness cost: 45.846815
[move_group-3] [INFO] [1732712677.840797995] [chomp_optimizer]: iteration: 10
[move_group-3] [INFO] [1732712677.841283341] [chomp_optimizer]: cCost 95.457955 over threshold 0.070000
[move_group-3] [INFO] [1732712677.850035222] [chomp_optimizer]: Collision cost 98.625313, smoothness cost: 45.695203
[move_group-3] [INFO] [1732712677.850093251] [chomp_optimizer]: cCost 98.625313 over threshold 0.070000
[move_group-3] [INFO] [1732712677.859182559] [chomp_optimizer]: Collision cost 94.799037, smoothness cost: 45.534362
[move_group-3] [INFO] [1732712677.859289310] [chomp_optimizer]: cCost 94.799037 over threshold 0.070000
[move_group-3] [INFO] [1732712677.868209929] [chomp_optimizer]: Collision cost 98.282816, smoothness cost: 45.389297
[move_group-3] [INFO] [1732712677.868280763] [chomp_optimizer]: cCost 98.282816 over threshold 0.070000
[move_group-3] [INFO] [1732712677.877146519] [chomp_optimizer]: Collision cost 102.671046, smoothness cost: 45.243865
[move_group-3] [INFO] [1732712677.877210249] [chomp_optimizer]: cCost 102.671046 over threshold 0.070000
[move_group-3] [INFO] [1732712677.885986025] [chomp_optimizer]: Collision cost 97.529442, smoothness cost: 45.080672
[move_group-3] [INFO] [1732712677.886041630] [chomp_optimizer]: cCost 97.529442 over threshold 0.070000
[move_group-3] [INFO] [1732712677.895451723] [chomp_optimizer]: Collision cost 101.322714, smoothness cost: 44.934556
[move_group-3] [INFO] [1732712677.895539829] [chomp_optimizer]: cCost 101.322714 over threshold 0.070000
[move_group-3] [INFO] [1732712677.904331425] [chomp_optimizer]: Collision cost 104.854334, smoothness cost: 44.789860
[move_group-3] [INFO] [1732712677.904391518] [chomp_optimizer]: cCost 104.854334 over threshold 0.070000
[move_group-3] [INFO] [1732712677.913225554] [chomp_optimizer]: Collision cost 101.243478, smoothness cost: 44.633964
[move_group-3] [INFO] [1732712677.913283273] [chomp_optimizer]: cCost 101.243478 over threshold 0.070000
[move_group-3] [INFO] [1732712677.922284585] [chomp_optimizer]: Collision cost 103.793240, smoothness cost: 44.490304
[move_group-3] [INFO] [1732712677.922422986] [chomp_optimizer]: cCost 103.793240 over threshold 0.070000
[move_group-3] [INFO] [1732712677.931636528] [chomp_optimizer]: Collision cost 99.815591, smoothness cost: 44.335133
[move_group-3] [INFO] [1732712677.931704446] [chomp_optimizer]: iteration: 20
[move_group-3] [INFO] [1732712677.932188039] [chomp_optimizer]: cCost 99.815591 over threshold 0.070000
[move_group-3] [INFO] [1732712677.941023738] [chomp_optimizer]: Collision cost 101.793288, smoothness cost: 44.192599
[move_group-3] [INFO] [1732712677.941086777] [chomp_optimizer]: cCost 101.793288 over threshold 0.070000
[move_group-3] [INFO] [1732712677.949826575] [chomp_optimizer]: Collision cost 103.935740, smoothness cost: 44.049623
[move_group-3] [INFO] [1732712677.949885977] [chomp_optimizer]: cCost 103.935740 over threshold 0.070000
[move_group-3] [INFO] [1732712677.959382063] [chomp_optimizer]: Collision cost 99.620132, smoothness cost: 43.894019
[move_group-3] [INFO] [1732712677.959468546] [chomp_optimizer]: cCost 99.620132 over threshold 0.070000
[move_group-3] [INFO] [1732712677.968285370] [chomp_optimizer]: Collision cost 101.350198, smoothness cost: 43.752833
[move_group-3] [INFO] [1732712677.968351424] [chomp_optimizer]: cCost 101.350198 over threshold 0.070000
[move_group-3] [INFO] [1732712677.977233461] [chomp_optimizer]: Collision cost 102.023825, smoothness cost: 43.611211
[move_group-3] [INFO] [1732712677.977306869] [chomp_optimizer]: cCost 102.023825 over threshold 0.070000
[move_group-3] [INFO] [1732712677.986466820] [chomp_optimizer]: Collision cost 98.606247, smoothness cost: 43.456599
[move_group-3] [INFO] [1732712677.986559635] [chomp_optimizer]: cCost 98.606247 over threshold 0.070000
[move_group-3] [INFO] [1732712677.995721510] [chomp_optimizer]: Collision cost 99.835015, smoothness cost: 43.317037
[move_group-3] [INFO] [1732712677.995795089] [chomp_optimizer]: cCost 99.835015 over threshold 0.070000
[move_group-3] [INFO] [1732712678.004521351] [chomp_optimizer]: Collision cost 100.621666, smoothness cost: 43.177073
[move_group-3] [INFO] [1732712678.004579461] [chomp_optimizer]: cCost 100.621666 over threshold 0.070000
[move_group-3] [INFO] [1732712678.013297387] [chomp_optimizer]: Collision cost 96.602869, smoothness cost: 43.023998
[move_group-3] [INFO] [1732712678.013344747] [chomp_optimizer]: cCost 96.602869 over threshold 0.070000
[move_group-3] [INFO] [1732712678.022688385] [chomp_optimizer]: Collision cost 97.660196, smoothness cost: 42.886161
[move_group-3] [INFO] [1732712678.022785849] [chomp_optimizer]: iteration: 30
[move_group-3] [INFO] [1732712678.023291123] [chomp_optimizer]: cCost 97.660196 over threshold 0.070000
[move_group-3] [INFO] [1732712678.032258219] [chomp_optimizer]: Collision cost 98.729535, smoothness cost: 42.747943
[move_group-3] [INFO] [1732712678.032326338] [chomp_optimizer]: cCost 98.729535 over threshold 0.070000
[move_group-3] [INFO] [1732712678.041145275] [chomp_optimizer]: Collision cost 98.973984, smoothness cost: 42.609488
[move_group-3] [INFO] [1732712678.041212201] [chomp_optimizer]: cCost 98.973984 over threshold 0.070000
[move_group-3] [INFO] [1732712678.050431645] [chomp_optimizer]: Collision cost 94.546044, smoothness cost: 42.457575
[move_group-3] [INFO] [1732712678.050529640] [chomp_optimizer]: cCost 94.546044 over threshold 0.070000
[move_group-3] [INFO] [1732712678.059611253] [chomp_optimizer]: Collision cost 95.517380, smoothness cost: 42.321686
[move_group-3] [INFO] [1732712678.059680905] [chomp_optimizer]: cCost 95.517380 over threshold 0.070000
[move_group-3] [INFO] [1732712678.068443185] [chomp_optimizer]: Collision cost 96.630911, smoothness cost: 42.185471
[move_group-3] [INFO] [1732712678.068495934] [chomp_optimizer]: cCost 96.630911 over threshold 0.070000
[move_group-3] [INFO] [1732712678.077318939] [chomp_optimizer]: Collision cost 96.442121, smoothness cost: 42.049086
[move_group-3] [INFO] [1732712678.077375446] [chomp_optimizer]: cCost 96.442121 over threshold 0.070000
[move_group-3] [INFO] [1732712678.086975518] [chomp_optimizer]: Collision cost 92.248347, smoothness cost: 41.899134
[move_group-3] [INFO] [1732712678.087067452] [chomp_optimizer]: cCost 92.248347 over threshold 0.070000
[move_group-3] [INFO] [1732712678.095964576] [chomp_optimizer]: Collision cost 93.066088, smoothness cost: 41.765411
[move_group-3] [INFO] [1732712678.096038406] [chomp_optimizer]: cCost 93.066088 over threshold 0.070000
[move_group-3] [INFO] [1732712678.104781470] [chomp_optimizer]: Collision cost 93.800542, smoothness cost: 41.631386
[move_group-3] [INFO] [1732712678.104838327] [chomp_optimizer]: cCost 93.800542 over threshold 0.070000
[move_group-3] [INFO] [1732712678.114332699] [chomp_optimizer]: Collision cost 93.515138, smoothness cost: 41.497213
[move_group-3] [INFO] [1732712678.114456493] [chomp_optimizer]: iteration: 40
[move_group-3] [INFO] [1732712678.114971515] [chomp_optimizer]: cCost 93.515138 over threshold 0.070000
[move_group-3] [INFO] [1732712678.124062226] [chomp_optimizer]: Collision cost 93.866399, smoothness cost: 41.363033
[move_group-3] [INFO] [1732712678.124138620] [chomp_optimizer]: cCost 93.866399 over threshold 0.070000
[move_group-3] [INFO] [1732712678.132884198] [chomp_optimizer]: Collision cost 90.255000, smoothness cost: 41.215175
[move_group-3] [INFO] [1732712678.132941406] [chomp_optimizer]: cCost 90.255000 over threshold 0.070000
[move_group-3] [INFO] [1732712678.141683138] [chomp_optimizer]: Collision cost 90.533940, smoothness cost: 41.083903
[move_group-3] [INFO] [1732712678.141777446] [chomp_optimizer]: cCost 90.533940 over threshold 0.070000
[move_group-3] [INFO] [1732712678.151069006] [chomp_optimizer]: Collision cost 91.190352, smoothness cost: 40.952388
[move_group-3] [INFO] [1732712678.151141062] [chomp_optimizer]: cCost 91.190352 over threshold 0.070000
[move_group-3] [INFO] [1732712678.159902580] [chomp_optimizer]: Collision cost 91.436356, smoothness cost: 40.820780
[move_group-3] [INFO] [1732712678.159965058] [chomp_optimizer]: cCost 91.436356 over threshold 0.070000
[move_group-3] [INFO] [1732712678.168740984] [chomp_optimizer]: Collision cost 91.548936, smoothness cost: 40.689204
[move_group-3] [INFO] [1732712678.168794796] [chomp_optimizer]: cCost 91.548936 over threshold 0.070000
[move_group-3] [INFO] [1732712678.178111994] [chomp_optimizer]: Collision cost 91.279797, smoothness cost: 40.557763
[move_group-3] [INFO] [1732712678.178212123] [chomp_optimizer]: cCost 91.279797 over threshold 0.070000
[move_group-3] [INFO] [1732712678.187075805] [chomp_optimizer]: Collision cost 90.675416, smoothness cost: 40.426534
[move_group-3] [INFO] [1732712678.187141378] [chomp_optimizer]: cCost 90.675416 over threshold 0.070000
[move_group-3] [INFO] [1732712678.195974492] [chomp_optimizer]: Collision cost 85.862978, smoothness cost: 40.281688
[move_group-3] [INFO] [1732712678.196041158] [chomp_optimizer]: cCost 85.862978 over threshold 0.070000
[move_group-3] [INFO] [1732712678.205039594] [chomp_optimizer]: Collision cost 86.923337, smoothness cost: 40.153642
[move_group-3] [INFO] [1732712678.205123502] [chomp_optimizer]: iteration: 50
[move_group-3] [INFO] [1732712678.205614569] [chomp_optimizer]: cCost 86.923337 over threshold 0.070000
[move_group-3] [INFO] [1732712678.214748682] [chomp_optimizer]: Collision cost 87.534282, smoothness cost: 40.025452
[move_group-3] [INFO] [1732712678.214822371] [chomp_optimizer]: cCost 87.534282 over threshold 0.070000
[move_group-3] [INFO] [1732712678.223610170] [chomp_optimizer]: Collision cost 87.310309, smoothness cost: 39.897253
[move_group-3] [INFO] [1732712678.223674812] [chomp_optimizer]: cCost 87.310309 over threshold 0.070000
[move_group-3] [INFO] [1732712678.232398970] [chomp_optimizer]: Collision cost 87.164747, smoothness cost: 39.768786
[move_group-3] [INFO] [1732712678.232449255] [chomp_optimizer]: cCost 87.164747 over threshold 0.070000
[move_group-3] [INFO] [1732712678.241735504] [chomp_optimizer]: Collision cost 87.098590, smoothness cost: 39.640590
[move_group-3] [INFO] [1732712678.241828680] [chomp_optimizer]: cCost 87.098590 over threshold 0.070000
[move_group-3] [INFO] [1732712678.250708543] [chomp_optimizer]: Collision cost 86.622502, smoothness cost: 39.512713
[move_group-3] [INFO] [1732712678.250773916] [chomp_optimizer]: cCost 86.622502 over threshold 0.070000
[move_group-3] [INFO] [1732712678.259505498] [chomp_optimizer]: Collision cost 86.063846, smoothness cost: 39.385192
[move_group-3] [INFO] [1732712678.259552507] [chomp_optimizer]: cCost 86.063846 over threshold 0.070000
[move_group-3] [INFO] [1732712678.268494126] [chomp_optimizer]: Collision cost 85.790478, smoothness cost: 39.258056
[move_group-3] [INFO] [1732712678.268573115] [chomp_optimizer]: cCost 85.790478 over threshold 0.070000
[move_group-3] [INFO] [1732712678.278066025] [chomp_optimizer]: Collision cost 85.515545, smoothness cost: 39.131327
[move_group-3] [INFO] [1732712678.278145374] [chomp_optimizer]: cCost 85.515545 over threshold 0.070000
[move_group-3] [INFO] [1732712678.286964552] [chomp_optimizer]: Collision cost 85.115331, smoothness cost: 39.005021
[move_group-3] [INFO] [1732712678.287022792] [chomp_optimizer]: cCost 85.115331 over threshold 0.070000
[move_group-3] [INFO] [1732712678.295702336] [chomp_optimizer]: Collision cost 83.808766, smoothness cost: 38.879154
[move_group-3] [INFO] [1732712678.295752390] [chomp_optimizer]: iteration: 60
[move_group-3] [INFO] [1732712678.296261301] [chomp_optimizer]: cCost 83.808766 over threshold 0.070000
[move_group-3] [INFO] [1732712678.305683457] [chomp_optimizer]: Collision cost 82.231006, smoothness cost: 38.753735
[move_group-3] [INFO] [1732712678.305777344] [chomp_optimizer]: cCost 82.231006 over threshold 0.070000
[move_group-3] [INFO] [1732712678.314594448] [chomp_optimizer]: Collision cost 82.056552, smoothness cost: 38.628773
[move_group-3] [INFO] [1732712678.314653710] [chomp_optimizer]: cCost 82.056552 over threshold 0.070000
[move_group-3] [INFO] [1732712678.323356388] [chomp_optimizer]: Collision cost 81.769893, smoothness cost: 38.504274
[move_group-3] [INFO] [1732712678.323404348] [chomp_optimizer]: cCost 81.769893 over threshold 0.070000
[move_group-3] [INFO] [1732712678.332353171] [chomp_optimizer]: Collision cost 81.440689, smoothness cost: 38.380243
[move_group-3] [INFO] [1732712678.332432460] [chomp_optimizer]: cCost 81.440689 over threshold 0.070000
[move_group-3] [INFO] [1732712678.341552586] [chomp_optimizer]: Collision cost 80.769037, smoothness cost: 38.256683
[move_group-3] [INFO] [1732712678.341626776] [chomp_optimizer]: cCost 80.769037 over threshold 0.070000
[move_group-3] [INFO] [1732712678.350407121] [chomp_optimizer]: Collision cost 80.370581, smoothness cost: 38.133598
[move_group-3] [INFO] [1732712678.350465962] [chomp_optimizer]: cCost 80.370581 over threshold 0.070000
[move_group-3] [INFO] [1732712678.359189429] [chomp_optimizer]: Collision cost 80.104895, smoothness cost: 38.010989
[move_group-3] [INFO] [1732712678.359238742] [chomp_optimizer]: cCost 80.104895 over threshold 0.070000
[move_group-3] [INFO] [1732712678.368489965] [chomp_optimizer]: Collision cost 79.436395, smoothness cost: 37.888857
[move_group-3] [INFO] [1732712678.368575296] [chomp_optimizer]: cCost 79.436395 over threshold 0.070000
[move_group-3] [INFO] [1732712678.377585264] [chomp_optimizer]: Collision cost 79.028409, smoothness cost: 37.767204
[move_group-3] [INFO] [1732712678.377659304] [chomp_optimizer]: cCost 79.028409 over threshold 0.070000
[move_group-3] [INFO] [1732712678.386382691] [chomp_optimizer]: Collision cost 77.506700, smoothness cost: 37.646030
[move_group-3] [INFO] [1732712678.386436372] [chomp_optimizer]: iteration: 70
[move_group-3] [INFO] [1732712678.386945113] [chomp_optimizer]: cCost 77.506700 over threshold 0.070000
[move_group-3] [INFO] [1732712678.396004343] [chomp_optimizer]: Collision cost 77.038742, smoothness cost: 37.525334
[move_group-3] [INFO] [1732712678.396085036] [chomp_optimizer]: cCost 77.038742 over threshold 0.070000
[move_group-3] [INFO] [1732712678.405199681] [chomp_optimizer]: Collision cost 76.697775, smoothness cost: 37.405117
[move_group-3] [INFO] [1732712678.405299910] [chomp_optimizer]: cCost 76.697775 over threshold 0.070000
[move_group-3] [INFO] [1732712678.414083231] [chomp_optimizer]: Collision cost 76.463184, smoothness cost: 37.285378
[move_group-3] [INFO] [1732712678.414140449] [chomp_optimizer]: cCost 76.463184 over threshold 0.070000
[move_group-3] [INFO] [1732712678.422882571] [chomp_optimizer]: Collision cost 75.773729, smoothness cost: 37.166116
[move_group-3] [INFO] [1732712678.422971198] [chomp_optimizer]: cCost 75.773729 over threshold 0.070000
[move_group-3] [INFO] [1732712678.432237249] [chomp_optimizer]: Collision cost 75.322429, smoothness cost: 37.047330
[move_group-3] [INFO] [1732712678.432308354] [chomp_optimizer]: cCost 75.322429 over threshold 0.070000
[move_group-3] [INFO] [1732712678.441125688] [chomp_optimizer]: Collision cost 74.857534, smoothness cost: 36.929020
[move_group-3] [INFO] [1732712678.441190961] [chomp_optimizer]: cCost 74.857534 over threshold 0.070000
[move_group-3] [INFO] [1732712678.449885824] [chomp_optimizer]: Collision cost 74.503659, smoothness cost: 36.810807
[move_group-3] [INFO] [1732712678.449936379] [chomp_optimizer]: cCost 74.503659 over threshold 0.070000
[move_group-3] [INFO] [1732712678.458962378] [chomp_optimizer]: Collision cost 74.173446, smoothness cost: 36.694433
[move_group-3] [INFO] [1732712678.459042799] [chomp_optimizer]: cCost 74.173446 over threshold 0.070000
[move_group-3] [INFO] [1732712678.468000949] [chomp_optimizer]: Collision cost 73.733235, smoothness cost: 36.578289
[move_group-3] [INFO] [1732712678.468070711] [chomp_optimizer]: cCost 73.733235 over threshold 0.070000
[move_group-3] [INFO] [1732712678.476899737] [chomp_optimizer]: Collision cost 73.248821, smoothness cost: 36.462424
[move_group-3] [INFO] [1732712678.476968197] [chomp_optimizer]: iteration: 80
[move_group-3] [INFO] [1732712678.477511352] [chomp_optimizer]: cCost 73.248821 over threshold 0.070000
[move_group-3] [INFO] [1732712678.486615718] [chomp_optimizer]: Collision cost 73.007067, smoothness cost: 36.346871
[move_group-3] [INFO] [1732712678.486801339] [chomp_optimizer]: cCost 73.007067 over threshold 0.070000
[move_group-3] [INFO] [1732712678.495993771] [chomp_optimizer]: Collision cost 73.005555, smoothness cost: 36.231659
[move_group-3] [INFO] [1732712678.496082047] [chomp_optimizer]: cCost 73.005555 over threshold 0.070000
[move_group-3] [INFO] [1732712678.504898831] [chomp_optimizer]: Collision cost 72.969035, smoothness cost: 36.116808
[move_group-3] [INFO] [1732712678.504956630] [chomp_optimizer]: cCost 72.969035 over threshold 0.070000
[move_group-3] [INFO] [1732712678.513674126] [chomp_optimizer]: Collision cost 72.612932, smoothness cost: 36.002336
[move_group-3] [INFO] [1732712678.513719431] [chomp_optimizer]: cCost 72.612932 over threshold 0.070000
[move_group-3] [INFO] [1732712678.522635271] [chomp_optimizer]: Collision cost 72.305145, smoothness cost: 35.888256
[move_group-3] [INFO] [1732712678.522712788] [chomp_optimizer]: cCost 72.305145 over threshold 0.070000
[move_group-3] [INFO] [1732712678.531712075] [chomp_optimizer]: Collision cost 72.215469, smoothness cost: 35.774578
[move_group-3] [INFO] [1732712678.531783109] [chomp_optimizer]: cCost 72.215469 over threshold 0.070000
[move_group-3] [INFO] [1732712678.540522056] [chomp_optimizer]: Collision cost 71.758018, smoothness cost: 35.661311
[move_group-3] [INFO] [1732712678.540575527] [chomp_optimizer]: cCost 71.758018 over threshold 0.070000
[move_group-3] [INFO] [1732712678.549411436] [chomp_optimizer]: Collision cost 70.860125, smoothness cost: 35.548460
[move_group-3] [INFO] [1732712678.549503480] [chomp_optimizer]: cCost 70.860125 over threshold 0.070000
[move_group-3] [INFO] [1732712678.558518828] [chomp_optimizer]: Collision cost 70.137617, smoothness cost: 35.436031
[move_group-3] [INFO] [1732712678.558630458] [chomp_optimizer]: cCost 70.137617 over threshold 0.070000
[move_group-3] [INFO] [1732712678.567535568] [chomp_optimizer]: Collision cost 70.125014, smoothness cost: 35.324029
[move_group-3] [INFO] [1732712678.567603567] [chomp_optimizer]: iteration: 90
[move_group-3] [INFO] [1732712678.568130642] [chomp_optimizer]: cCost 70.125014 over threshold 0.070000
[move_group-3] [INFO] [1732712678.576896699] [chomp_optimizer]: Collision cost 69.960501, smoothness cost: 35.212455
[move_group-3] [INFO] [1732712678.576957644] [chomp_optimizer]: cCost 69.960501 over threshold 0.070000
[move_group-3] [INFO] [1732712678.586068873] [chomp_optimizer]: Collision cost 69.702705, smoothness cost: 35.101312
[move_group-3] [INFO] [1732712678.586153543] [chomp_optimizer]: cCost 69.702705 over threshold 0.070000
[move_group-3] [INFO] [1732712678.595226079] [chomp_optimizer]: Collision cost 69.206306, smoothness cost: 34.990601
[move_group-3] [INFO] [1732712678.595303786] [chomp_optimizer]: cCost 69.206306 over threshold 0.070000
[move_group-3] [INFO] [1732712678.604052370] [chomp_optimizer]: Collision cost 69.003935, smoothness cost: 34.880323
[move_group-3] [INFO] [1732712678.604107795] [chomp_optimizer]: cCost 69.003935 over threshold 0.070000
[move_group-3] [INFO] [1732712678.612896204] [chomp_optimizer]: Collision cost 68.613563, smoothness cost: 34.770479
[move_group-3] [INFO] [1732712678.612972158] [chomp_optimizer]: cCost 68.613563 over threshold 0.070000
[move_group-3] [INFO] [1732712678.621815992] [chomp_optimizer]: Collision cost 68.187433, smoothness cost: 34.661068
[move_group-3] [INFO] [1732712678.621889220] [chomp_optimizer]: cCost 68.187433 over threshold 0.070000
[move_group-3] [INFO] [1732712678.630839425] [chomp_optimizer]: Collision cost 67.459156, smoothness cost: 34.552091
[move_group-3] [INFO] [1732712678.630899298] [chomp_optimizer]: cCost 67.459156 over threshold 0.070000
[move_group-3] [INFO] [1732712678.639662841] [chomp_optimizer]: Collision cost 67.202471, smoothness cost: 34.443547
[move_group-3] [INFO] [1732712678.639733434] [chomp_optimizer]: cCost 67.202471 over threshold 0.070000
[move_group-3] [INFO] [1732712678.649053507] [chomp_optimizer]: Collision cost 67.131498, smoothness cost: 34.335435
[move_group-3] [INFO] [1732712678.649141514] [chomp_optimizer]: cCost 67.131498 over threshold 0.070000
[move_group-3] [INFO] [1732712678.658115834] [chomp_optimizer]: Collision cost 66.847500, smoothness cost: 34.227754
[move_group-3] [INFO] [1732712678.658191166] [chomp_optimizer]: iteration: 100
[move_group-3] [INFO] [1732712678.660218834] [chomp_optimizer]: cCost 66.847500 over threshold 0.070000
[move_group-3] [INFO] [1732712678.669215817] [chomp_optimizer]: Collision cost 66.404303, smoothness cost: 34.120503
[move_group-3] [INFO] [1732712678.669292332] [chomp_optimizer]: cCost 66.404303 over threshold 0.070000
[move_group-3] [INFO] [1732712678.678404913] [chomp_optimizer]: Collision cost 65.742026, smoothness cost: 34.013682
[move_group-3] [INFO] [1732712678.678528988] [chomp_optimizer]: cCost 65.742026 over threshold 0.070000
[move_group-3] [INFO] [1732712678.687391247] [chomp_optimizer]: Collision cost 65.375393, smoothness cost: 33.907289
[move_group-3] [INFO] [1732712678.687484302] [chomp_optimizer]: cCost 65.375393 over threshold 0.070000
[move_group-3] [INFO] [1732712678.696461027] [chomp_optimizer]: Collision cost 65.146529, smoothness cost: 33.801322
[move_group-3] [INFO] [1732712678.696533584] [chomp_optimizer]: cCost 65.146529 over threshold 0.070000
[move_group-3] [INFO] [1732712678.705361609] [chomp_optimizer]: Collision cost 64.782350, smoothness cost: 33.695780
[move_group-3] [INFO] [1732712678.705494269] [chomp_optimizer]: cCost 64.782350 over threshold 0.070000
[move_group-3] [INFO] [1732712678.714644802] [chomp_optimizer]: Collision cost 64.247481, smoothness cost: 33.590662
[move_group-3] [INFO] [1732712678.714704084] [chomp_optimizer]: cCost 64.247481 over threshold 0.070000
[move_group-3] [INFO] [1732712678.723565532] [chomp_optimizer]: Collision cost 64.261467, smoothness cost: 33.485966
[move_group-3] [INFO] [1732712678.723630575] [chomp_optimizer]: cCost 64.261467 over threshold 0.070000
[move_group-3] [INFO] [1732712678.732435505] [chomp_optimizer]: Collision cost 63.879441, smoothness cost: 33.381691
[move_group-3] [INFO] [1732712678.732493956] [chomp_optimizer]: cCost 63.879441 over threshold 0.070000
[move_group-3] [INFO] [1732712678.741576731] [chomp_optimizer]: Collision cost 63.588735, smoothness cost: 33.277834
[move_group-3] [INFO] [1732712678.741661651] [chomp_optimizer]: cCost 63.588735 over threshold 0.070000
[move_group-3] [INFO] [1732712678.750555590] [chomp_optimizer]: Collision cost 63.308438, smoothness cost: 33.174395
[move_group-3] [INFO] [1732712678.750628708] [chomp_optimizer]: iteration: 110
[move_group-3] [INFO] [1732712678.751358386] [chomp_optimizer]: Chomp Got mesh to mesh safety at iter 110. Breaking out early.
[move_group-3] [INFO] [1732712678.751366451] [chomp_optimizer]: cCost 63.308438 over threshold 0.070000
[move_group-3] [INFO] [1732712678.751370939] [chomp_optimizer]: Chomp path is collision free
[move_group-3] [INFO] [1732712678.751374556] [chomp_optimizer]: Terminated after 111 iterations, using path from iteration 110
[move_group-3] [INFO] [1732712678.751377452] [chomp_optimizer]: Optimization core finished in 1.012217 sec
[move_group-3] [INFO] [1732712678.751382912] [chomp_optimizer]: Time per iteration 0.009119 sec
[move_group-3] [INFO] [1732712678.751386930] [chomp_planner]: Planned with Chomp Parameters (learning_rate, ridge_factor, planning_time_limit, max_iterations), attempt: # 1 
[move_group-3] [INFO] [1732712678.751391148] [chomp_planner]: Learning rate: 0.010000 ridge factor: 0.010000 planning time limit: 10.000000 max_iterations 200 
[move_group-3] [WARN] [1732712678.751616733] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.
[move_group-3] [ERROR] [1732712678.769967864] [moveit.ros_planning.planning_pipeline]: Computed path is not valid. Invalid states at index locations: [ 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 ] out of 109. Explanations follow in command line. Contacts are published on /display_contacts
[move_group-3] [INFO] [1732712678.769995246] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.114623, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770018209] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.143131, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770033899] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.171640, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770049158] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.199959, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770064066] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.223781, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770110073] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.241320, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770126654] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.252578, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770141883] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.257578, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770157312] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.257009, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770172411] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.252813, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770187699] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.242480, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770202598] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.225255, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770217315] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.201138, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770231963] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.170865, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770246190] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.139593, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [INFO] [1732712678.770260156] [moveit_kinematic_constraints.kinematic_constraints]: Constraint violated:: Joint name: 'joint_1', actual value: -0.108321, desired value: 0.000000, tolerance_above: 1.658060, tolerance_below: 0.087266
[move_group-3] [ERROR] [1732712678.770268903] [moveit.ros_planning.planning_pipeline]: Completed listing of explanations for invalid states.
[move_group-3] [INFO] [1732712678.770625406] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.

Can somebody confirm or deny this behavior, please?
@aizmailovs aizmailovs added the bug Something isn't working label Nov 27, 2024
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
bug Something isn't working
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
1 participant
@aizmailovs